View Javadoc
1   package com.irurueta.navigation.inertial.calibration.accelerometer;
2   
3   import com.irurueta.algebra.Matrix;
4   import com.irurueta.algebra.WrongSizeException;
5   import com.irurueta.navigation.LockedException;
6   import com.irurueta.navigation.NotReadyException;
7   import com.irurueta.navigation.frames.CoordinateTransformation;
8   import com.irurueta.navigation.frames.ECEFFrame;
9   import com.irurueta.navigation.frames.FrameType;
10  import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
11  import com.irurueta.navigation.frames.NEDFrame;
12  import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
13  import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
14  import com.irurueta.navigation.geodesic.Constants;
15  import com.irurueta.navigation.inertial.BodyKinematics;
16  import com.irurueta.navigation.inertial.ECEFGravity;
17  import com.irurueta.navigation.inertial.ECEFPosition;
18  import com.irurueta.navigation.inertial.ECEFVelocity;
19  import com.irurueta.navigation.inertial.NEDPosition;
20  import com.irurueta.navigation.inertial.NEDVelocity;
21  import com.irurueta.navigation.inertial.calibration.BodyKinematicsGenerator;
22  import com.irurueta.navigation.inertial.calibration.CalibrationException;
23  import com.irurueta.navigation.inertial.calibration.IMUErrors;
24  import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
25  import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
26  import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
27  import com.irurueta.statistics.UniformRandomizer;
28  import com.irurueta.units.Acceleration;
29  import com.irurueta.units.AccelerationUnit;
30  import org.junit.Test;
31  
32  import java.util.ArrayList;
33  import java.util.Collection;
34  import java.util.Collections;
35  import java.util.List;
36  import java.util.Random;
37  
38  import static org.junit.Assert.*;
39  
40  public class KnownPositionAccelerometerCalibratorTest implements
41          KnownPositionAccelerometerCalibratorListener {
42  
43      private static final double TIME_INTERVAL_SECONDS = 0.02;
44  
45      private static final double MICRO_G_TO_METERS_PER_SECOND_SQUARED = 9.80665E-6;
46      private static final double DEG_TO_RAD = 0.01745329252;
47  
48      private static final double MIN_ANGLE_DEGREES = -180.0;
49      private static final double MAX_ANGLE_DEGREES = 180.0;
50  
51      private static final double MIN_LATITUDE_DEGREES = -90.0;
52      private static final double MAX_LATITUDE_DEGREEs = 90.0;
53      private static final double MIN_LONGITUDE_DEGREES = -180.0;
54      private static final double MAX_LONGITUDE_DEGREES = 180.0;
55      private static final double MIN_HEIGHT = -50.0;
56      private static final double MAX_HEIGHT = 50.0;
57  
58      private static final int LARGE_MEASUREMENT_NUMBER = 100000;
59  
60      private static final double ABSOLUTE_ERROR = 1e-8;
61      private static final double LARGE_ABSOLUTE_ERROR = 5e-5;
62      private static final double VERY_LARGE_ABSOLUTE_ERROR = 1e-3;
63  
64      private static final int TIMES = 100;
65  
66      private int mCalibrateStart;
67      private int mCalibrateEnd;
68  
69      @Test
70      public void testConstructor1() throws WrongSizeException {
71          final KnownPositionAccelerometerCalibrator calibrator =
72                  new KnownPositionAccelerometerCalibrator();
73  
74          // check default values
75          assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
76          assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
77          assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
78          final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
79          assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
80          assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
81          final Acceleration bx2 = new Acceleration(0.0,
82                  AccelerationUnit.FEET_PER_SQUARED_SECOND);
83          calibrator.getInitialBiasXAsAcceleration(bx2);
84          assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
85          assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
86          final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
87          assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
88          assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
89          final Acceleration by2 = new Acceleration(0.0,
90                  AccelerationUnit.FEET_PER_SQUARED_SECOND);
91          calibrator.getInitialBiasYAsAcceleration(by2);
92          assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
93          assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
94          final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
95          assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
96          assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
97          final Acceleration bz2 = new Acceleration(0.0,
98                  AccelerationUnit.FEET_PER_SQUARED_SECOND);
99          calibrator.getInitialBiasZAsAcceleration(bz2);
100         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
101         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
102         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
103         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
104         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
105         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
106         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
107         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
108         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
109         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
110         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
111         final double[] bias1 = calibrator.getInitialBias();
112         assertArrayEquals(bias1, new double[3], 0.0);
113         final double[] bias2 = new double[3];
114         calibrator.getInitialBias(bias2);
115         assertArrayEquals(bias1, bias2, 0.0);
116         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
117         assertEquals(b1, new Matrix(3, 1));
118         final Matrix b2 = new Matrix(3, 1);
119         calibrator.getInitialBiasAsMatrix(b2);
120         assertEquals(b1, b2);
121         final Matrix ma1 = calibrator.getInitialMa();
122         assertEquals(ma1, new Matrix(3, 3));
123         final Matrix ma2 = new Matrix(3, 3);
124         calibrator.getInitialMa(ma2);
125         assertEquals(ma1, ma2);
126         assertNull(calibrator.getEcefPosition());
127         assertNull(calibrator.getNedPosition());
128         assertFalse(calibrator.getNedPosition(null));
129         assertNull(calibrator.getMeasurements());
130         assertFalse(calibrator.isCommonAxisUsed());
131         assertNull(calibrator.getListener());
132         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
133         assertFalse(calibrator.isReady());
134         assertFalse(calibrator.isRunning());
135         assertNull(calibrator.getEstimatedBiases());
136         assertFalse(calibrator.getEstimatedBiases(null));
137         assertNull(calibrator.getEstimatedBiasesAsMatrix());
138         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
139         assertNull(calibrator.getEstimatedBiasFx());
140         assertNull(calibrator.getEstimatedBiasFy());
141         assertNull(calibrator.getEstimatedBiasFz());
142         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
143         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
144         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
145         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
146         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
147         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
148         assertNull(calibrator.getEstimatedMa());
149         assertNull(calibrator.getEstimatedSx());
150         assertNull(calibrator.getEstimatedSy());
151         assertNull(calibrator.getEstimatedSz());
152         assertNull(calibrator.getEstimatedMxy());
153         assertNull(calibrator.getEstimatedMxz());
154         assertNull(calibrator.getEstimatedMyx());
155         assertNull(calibrator.getEstimatedMyz());
156         assertNull(calibrator.getEstimatedMzx());
157         assertNull(calibrator.getEstimatedMzy());
158         assertNull(calibrator.getEstimatedCovariance());
159         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
160         assertNull(calibrator.getGroundTruthGravityNorm());
161         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
162         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
163     }
164 
165     @Test
166     public void testConstructor2() throws WrongSizeException {
167         final KnownPositionAccelerometerCalibrator calibrator =
168                 new KnownPositionAccelerometerCalibrator(this);
169 
170         // check default values
171         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
172         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
173         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
174         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
175         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
176         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
177         final Acceleration bx2 = new Acceleration(0.0,
178                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
179         calibrator.getInitialBiasXAsAcceleration(bx2);
180         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
181         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
182         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
183         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
184         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
185         final Acceleration by2 = new Acceleration(0.0,
186                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
187         calibrator.getInitialBiasYAsAcceleration(by2);
188         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
189         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
190         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
191         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
192         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
193         final Acceleration bz2 = new Acceleration(0.0,
194                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
195         calibrator.getInitialBiasZAsAcceleration(bz2);
196         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
197         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
198         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
199         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
200         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
201         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
202         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
203         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
204         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
205         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
206         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
207         final double[] bias1 = calibrator.getInitialBias();
208         assertArrayEquals(bias1, new double[3], 0.0);
209         final double[] bias2 = new double[3];
210         calibrator.getInitialBias(bias2);
211         assertArrayEquals(bias1, bias2, 0.0);
212         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
213         assertEquals(b1, new Matrix(3, 1));
214         final Matrix b2 = new Matrix(3, 1);
215         calibrator.getInitialBiasAsMatrix(b2);
216         assertEquals(b1, b2);
217         final Matrix ma1 = calibrator.getInitialMa();
218         assertEquals(ma1, new Matrix(3, 3));
219         final Matrix ma2 = new Matrix(3, 3);
220         calibrator.getInitialMa(ma2);
221         assertEquals(ma1, ma2);
222         assertNull(calibrator.getEcefPosition());
223         assertNull(calibrator.getNedPosition());
224         assertFalse(calibrator.getNedPosition(null));
225         assertNull(calibrator.getMeasurements());
226         assertFalse(calibrator.isCommonAxisUsed());
227         assertSame(calibrator.getListener(), this);
228         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
229         assertFalse(calibrator.isReady());
230         assertFalse(calibrator.isRunning());
231         assertNull(calibrator.getEstimatedBiases());
232         assertFalse(calibrator.getEstimatedBiases(null));
233         assertNull(calibrator.getEstimatedBiasesAsMatrix());
234         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
235         assertNull(calibrator.getEstimatedBiasFx());
236         assertNull(calibrator.getEstimatedBiasFy());
237         assertNull(calibrator.getEstimatedBiasFz());
238         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
239         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
240         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
241         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
242         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
243         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
244         assertNull(calibrator.getEstimatedMa());
245         assertNull(calibrator.getEstimatedSx());
246         assertNull(calibrator.getEstimatedSy());
247         assertNull(calibrator.getEstimatedSz());
248         assertNull(calibrator.getEstimatedMxy());
249         assertNull(calibrator.getEstimatedMxz());
250         assertNull(calibrator.getEstimatedMyx());
251         assertNull(calibrator.getEstimatedMyz());
252         assertNull(calibrator.getEstimatedMzx());
253         assertNull(calibrator.getEstimatedMzy());
254         assertNull(calibrator.getEstimatedCovariance());
255         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
256         assertNull(calibrator.getGroundTruthGravityNorm());
257         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
258         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
259     }
260 
261     @Test
262     public void testConstructor3() throws WrongSizeException {
263         final Collection<StandardDeviationBodyKinematics> measurements =
264                 Collections.emptyList();
265         final KnownPositionAccelerometerCalibrator calibrator =
266                 new KnownPositionAccelerometerCalibrator(measurements);
267 
268         // check default values
269         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
270         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
271         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
272         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
273         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
274         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
275         final Acceleration bx2 = new Acceleration(0.0,
276                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
277         calibrator.getInitialBiasXAsAcceleration(bx2);
278         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
279         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
280         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
281         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
282         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
283         final Acceleration by2 = new Acceleration(0.0,
284                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
285         calibrator.getInitialBiasYAsAcceleration(by2);
286         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
287         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
288         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
289         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
290         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
291         final Acceleration bz2 = new Acceleration(0.0,
292                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
293         calibrator.getInitialBiasZAsAcceleration(bz2);
294         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
295         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
296         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
297         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
298         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
299         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
300         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
301         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
302         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
303         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
304         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
305         final double[] bias1 = calibrator.getInitialBias();
306         assertArrayEquals(bias1, new double[3], 0.0);
307         final double[] bias2 = new double[3];
308         calibrator.getInitialBias(bias2);
309         assertArrayEquals(bias1, bias2, 0.0);
310         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
311         assertEquals(b1, new Matrix(3, 1));
312         final Matrix b2 = new Matrix(3, 1);
313         calibrator.getInitialBiasAsMatrix(b2);
314         assertEquals(b1, b2);
315         final Matrix ma1 = calibrator.getInitialMa();
316         assertEquals(ma1, new Matrix(3, 3));
317         final Matrix ma2 = new Matrix(3, 3);
318         calibrator.getInitialMa(ma2);
319         assertEquals(ma1, ma2);
320         assertNull(calibrator.getEcefPosition());
321         assertNull(calibrator.getNedPosition());
322         assertFalse(calibrator.getNedPosition(null));
323         assertSame(calibrator.getMeasurements(), measurements);
324         assertFalse(calibrator.isCommonAxisUsed());
325         assertNull(calibrator.getListener());
326         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
327         assertFalse(calibrator.isReady());
328         assertFalse(calibrator.isRunning());
329         assertNull(calibrator.getEstimatedBiases());
330         assertFalse(calibrator.getEstimatedBiases(null));
331         assertNull(calibrator.getEstimatedBiasesAsMatrix());
332         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
333         assertNull(calibrator.getEstimatedBiasFx());
334         assertNull(calibrator.getEstimatedBiasFy());
335         assertNull(calibrator.getEstimatedBiasFz());
336         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
337         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
338         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
339         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
340         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
341         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
342         assertNull(calibrator.getEstimatedMa());
343         assertNull(calibrator.getEstimatedSx());
344         assertNull(calibrator.getEstimatedSy());
345         assertNull(calibrator.getEstimatedSz());
346         assertNull(calibrator.getEstimatedMxy());
347         assertNull(calibrator.getEstimatedMxz());
348         assertNull(calibrator.getEstimatedMyx());
349         assertNull(calibrator.getEstimatedMyz());
350         assertNull(calibrator.getEstimatedMzx());
351         assertNull(calibrator.getEstimatedMzy());
352         assertNull(calibrator.getEstimatedCovariance());
353         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
354         assertNull(calibrator.getGroundTruthGravityNorm());
355         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
356         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
357     }
358 
359     @Test
360     public void testConstructor4() throws WrongSizeException {
361         final Collection<StandardDeviationBodyKinematics> measurements =
362                 Collections.emptyList();
363         final KnownPositionAccelerometerCalibrator calibrator =
364                 new KnownPositionAccelerometerCalibrator(measurements, this);
365 
366         // check default values
367         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
368         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
369         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
370         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
371         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
372         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
373         final Acceleration bx2 = new Acceleration(0.0,
374                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
375         calibrator.getInitialBiasXAsAcceleration(bx2);
376         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
377         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
378         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
379         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
380         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
381         final Acceleration by2 = new Acceleration(0.0,
382                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
383         calibrator.getInitialBiasYAsAcceleration(by2);
384         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
385         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
386         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
387         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
388         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
389         final Acceleration bz2 = new Acceleration(0.0,
390                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
391         calibrator.getInitialBiasZAsAcceleration(bz2);
392         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
393         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
394         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
395         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
396         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
397         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
398         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
399         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
400         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
401         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
402         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
403         final double[] bias1 = calibrator.getInitialBias();
404         assertArrayEquals(bias1, new double[3], 0.0);
405         final double[] bias2 = new double[3];
406         calibrator.getInitialBias(bias2);
407         assertArrayEquals(bias1, bias2, 0.0);
408         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
409         assertEquals(b1, new Matrix(3, 1));
410         final Matrix b2 = new Matrix(3, 1);
411         calibrator.getInitialBiasAsMatrix(b2);
412         assertEquals(b1, b2);
413         final Matrix ma1 = calibrator.getInitialMa();
414         assertEquals(ma1, new Matrix(3, 3));
415         final Matrix ma2 = new Matrix(3, 3);
416         calibrator.getInitialMa(ma2);
417         assertEquals(ma1, ma2);
418         assertNull(calibrator.getEcefPosition());
419         assertNull(calibrator.getNedPosition());
420         assertFalse(calibrator.getNedPosition(null));
421         assertSame(calibrator.getMeasurements(), measurements);
422         assertFalse(calibrator.isCommonAxisUsed());
423         assertSame(calibrator.getListener(), this);
424         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
425         assertFalse(calibrator.isReady());
426         assertFalse(calibrator.isRunning());
427         assertNull(calibrator.getEstimatedBiases());
428         assertFalse(calibrator.getEstimatedBiases(null));
429         assertNull(calibrator.getEstimatedBiasesAsMatrix());
430         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
431         assertNull(calibrator.getEstimatedBiasFx());
432         assertNull(calibrator.getEstimatedBiasFy());
433         assertNull(calibrator.getEstimatedBiasFz());
434         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
435         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
436         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
437         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
438         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
439         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
440         assertNull(calibrator.getEstimatedMa());
441         assertNull(calibrator.getEstimatedSx());
442         assertNull(calibrator.getEstimatedSy());
443         assertNull(calibrator.getEstimatedSz());
444         assertNull(calibrator.getEstimatedMxy());
445         assertNull(calibrator.getEstimatedMxz());
446         assertNull(calibrator.getEstimatedMyx());
447         assertNull(calibrator.getEstimatedMyz());
448         assertNull(calibrator.getEstimatedMzx());
449         assertNull(calibrator.getEstimatedMzy());
450         assertNull(calibrator.getEstimatedCovariance());
451         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
452         assertNull(calibrator.getGroundTruthGravityNorm());
453         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
454         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
455     }
456 
457     @Test
458     public void testConstructor5() throws WrongSizeException {
459         final KnownPositionAccelerometerCalibrator calibrator =
460                 new KnownPositionAccelerometerCalibrator(true);
461 
462         // check default values
463         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
464         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
465         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
466         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
467         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
468         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
469         final Acceleration bx2 = new Acceleration(0.0,
470                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
471         calibrator.getInitialBiasXAsAcceleration(bx2);
472         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
473         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
474         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
475         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
476         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
477         final Acceleration by2 = new Acceleration(0.0,
478                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
479         calibrator.getInitialBiasYAsAcceleration(by2);
480         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
481         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
482         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
483         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
484         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
485         final Acceleration bz2 = new Acceleration(0.0,
486                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
487         calibrator.getInitialBiasZAsAcceleration(bz2);
488         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
489         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
490         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
491         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
492         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
493         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
494         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
495         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
496         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
497         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
498         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
499         final double[] bias1 = calibrator.getInitialBias();
500         assertArrayEquals(bias1, new double[3], 0.0);
501         final double[] bias2 = new double[3];
502         calibrator.getInitialBias(bias2);
503         assertArrayEquals(bias1, bias2, 0.0);
504         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
505         assertEquals(b1, new Matrix(3, 1));
506         final Matrix b2 = new Matrix(3, 1);
507         calibrator.getInitialBiasAsMatrix(b2);
508         assertEquals(b1, b2);
509         final Matrix ma1 = calibrator.getInitialMa();
510         assertEquals(ma1, new Matrix(3, 3));
511         final Matrix ma2 = new Matrix(3, 3);
512         calibrator.getInitialMa(ma2);
513         assertEquals(ma1, ma2);
514         assertNull(calibrator.getEcefPosition());
515         assertNull(calibrator.getNedPosition());
516         assertFalse(calibrator.getNedPosition(null));
517         assertNull(calibrator.getMeasurements());
518         assertTrue(calibrator.isCommonAxisUsed());
519         assertNull(calibrator.getListener());
520         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
521         assertFalse(calibrator.isReady());
522         assertFalse(calibrator.isRunning());
523         assertNull(calibrator.getEstimatedBiases());
524         assertFalse(calibrator.getEstimatedBiases(null));
525         assertNull(calibrator.getEstimatedBiasesAsMatrix());
526         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
527         assertNull(calibrator.getEstimatedBiasFx());
528         assertNull(calibrator.getEstimatedBiasFy());
529         assertNull(calibrator.getEstimatedBiasFz());
530         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
531         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
532         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
533         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
534         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
535         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
536         assertNull(calibrator.getEstimatedMa());
537         assertNull(calibrator.getEstimatedSx());
538         assertNull(calibrator.getEstimatedSy());
539         assertNull(calibrator.getEstimatedSz());
540         assertNull(calibrator.getEstimatedMxy());
541         assertNull(calibrator.getEstimatedMxz());
542         assertNull(calibrator.getEstimatedMyx());
543         assertNull(calibrator.getEstimatedMyz());
544         assertNull(calibrator.getEstimatedMzx());
545         assertNull(calibrator.getEstimatedMzy());
546         assertNull(calibrator.getEstimatedCovariance());
547         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
548         assertNull(calibrator.getGroundTruthGravityNorm());
549         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
550         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
551     }
552 
553     @Test
554     public void testConstructor6() throws WrongSizeException {
555         final KnownPositionAccelerometerCalibrator calibrator =
556                 new KnownPositionAccelerometerCalibrator(true,
557                         this);
558 
559         // check default values
560         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
561         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
562         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
563         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
564         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
565         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
566         final Acceleration bx2 = new Acceleration(0.0,
567                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
568         calibrator.getInitialBiasXAsAcceleration(bx2);
569         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
570         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
571         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
572         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
573         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
574         final Acceleration by2 = new Acceleration(0.0,
575                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
576         calibrator.getInitialBiasYAsAcceleration(by2);
577         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
578         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
579         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
580         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
581         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
582         final Acceleration bz2 = new Acceleration(0.0,
583                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
584         calibrator.getInitialBiasZAsAcceleration(bz2);
585         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
586         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
587         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
588         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
589         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
590         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
591         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
592         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
593         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
594         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
595         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
596         final double[] bias1 = calibrator.getInitialBias();
597         assertArrayEquals(bias1, new double[3], 0.0);
598         final double[] bias2 = new double[3];
599         calibrator.getInitialBias(bias2);
600         assertArrayEquals(bias1, bias2, 0.0);
601         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
602         assertEquals(b1, new Matrix(3, 1));
603         final Matrix b2 = new Matrix(3, 1);
604         calibrator.getInitialBiasAsMatrix(b2);
605         assertEquals(b1, b2);
606         final Matrix ma1 = calibrator.getInitialMa();
607         assertEquals(ma1, new Matrix(3, 3));
608         final Matrix ma2 = new Matrix(3, 3);
609         calibrator.getInitialMa(ma2);
610         assertEquals(ma1, ma2);
611         assertNull(calibrator.getEcefPosition());
612         assertNull(calibrator.getNedPosition());
613         assertFalse(calibrator.getNedPosition(null));
614         assertNull(calibrator.getMeasurements());
615         assertTrue(calibrator.isCommonAxisUsed());
616         assertSame(calibrator.getListener(), this);
617         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
618         assertFalse(calibrator.isReady());
619         assertFalse(calibrator.isRunning());
620         assertNull(calibrator.getEstimatedBiases());
621         assertFalse(calibrator.getEstimatedBiases(null));
622         assertNull(calibrator.getEstimatedBiasesAsMatrix());
623         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
624         assertNull(calibrator.getEstimatedBiasFx());
625         assertNull(calibrator.getEstimatedBiasFy());
626         assertNull(calibrator.getEstimatedBiasFz());
627         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
628         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
629         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
630         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
631         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
632         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
633         assertNull(calibrator.getEstimatedMa());
634         assertNull(calibrator.getEstimatedSx());
635         assertNull(calibrator.getEstimatedSy());
636         assertNull(calibrator.getEstimatedSz());
637         assertNull(calibrator.getEstimatedMxy());
638         assertNull(calibrator.getEstimatedMxz());
639         assertNull(calibrator.getEstimatedMyx());
640         assertNull(calibrator.getEstimatedMyz());
641         assertNull(calibrator.getEstimatedMzx());
642         assertNull(calibrator.getEstimatedMzy());
643         assertNull(calibrator.getEstimatedCovariance());
644         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
645         assertNull(calibrator.getGroundTruthGravityNorm());
646         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
647         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
648     }
649 
650     @Test
651     public void testConstructor7() throws WrongSizeException {
652         final Collection<StandardDeviationBodyKinematics> measurements =
653                 Collections.emptyList();
654         final KnownPositionAccelerometerCalibrator calibrator =
655                 new KnownPositionAccelerometerCalibrator(measurements,
656                         true);
657 
658         // check default values
659         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
660         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
661         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
662         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
663         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
664         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
665         final Acceleration bx2 = new Acceleration(0.0,
666                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
667         calibrator.getInitialBiasXAsAcceleration(bx2);
668         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
669         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
670         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
671         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
672         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
673         final Acceleration by2 = new Acceleration(0.0,
674                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
675         calibrator.getInitialBiasYAsAcceleration(by2);
676         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
677         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
678         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
679         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
680         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
681         final Acceleration bz2 = new Acceleration(0.0,
682                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
683         calibrator.getInitialBiasZAsAcceleration(bz2);
684         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
685         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
686         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
687         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
688         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
689         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
690         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
691         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
692         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
693         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
694         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
695         final double[] bias1 = calibrator.getInitialBias();
696         assertArrayEquals(bias1, new double[3], 0.0);
697         final double[] bias2 = new double[3];
698         calibrator.getInitialBias(bias2);
699         assertArrayEquals(bias1, bias2, 0.0);
700         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
701         assertEquals(b1, new Matrix(3, 1));
702         final Matrix b2 = new Matrix(3, 1);
703         calibrator.getInitialBiasAsMatrix(b2);
704         assertEquals(b1, b2);
705         final Matrix ma1 = calibrator.getInitialMa();
706         assertEquals(ma1, new Matrix(3, 3));
707         final Matrix ma2 = new Matrix(3, 3);
708         calibrator.getInitialMa(ma2);
709         assertEquals(ma1, ma2);
710         assertNull(calibrator.getEcefPosition());
711         assertNull(calibrator.getNedPosition());
712         assertFalse(calibrator.getNedPosition(null));
713         assertSame(calibrator.getMeasurements(), measurements);
714         assertTrue(calibrator.isCommonAxisUsed());
715         assertNull(calibrator.getListener());
716         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
717         assertFalse(calibrator.isReady());
718         assertFalse(calibrator.isRunning());
719         assertNull(calibrator.getEstimatedBiases());
720         assertFalse(calibrator.getEstimatedBiases(null));
721         assertNull(calibrator.getEstimatedBiasesAsMatrix());
722         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
723         assertNull(calibrator.getEstimatedBiasFx());
724         assertNull(calibrator.getEstimatedBiasFy());
725         assertNull(calibrator.getEstimatedBiasFz());
726         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
727         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
728         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
729         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
730         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
731         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
732         assertNull(calibrator.getEstimatedMa());
733         assertNull(calibrator.getEstimatedSx());
734         assertNull(calibrator.getEstimatedSy());
735         assertNull(calibrator.getEstimatedSz());
736         assertNull(calibrator.getEstimatedMxy());
737         assertNull(calibrator.getEstimatedMxz());
738         assertNull(calibrator.getEstimatedMyx());
739         assertNull(calibrator.getEstimatedMyz());
740         assertNull(calibrator.getEstimatedMzx());
741         assertNull(calibrator.getEstimatedMzy());
742         assertNull(calibrator.getEstimatedCovariance());
743         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
744         assertNull(calibrator.getGroundTruthGravityNorm());
745         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
746         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
747     }
748 
749     @Test
750     public void testConstructor8() throws WrongSizeException {
751         final Collection<StandardDeviationBodyKinematics> measurements =
752                 Collections.emptyList();
753         final KnownPositionAccelerometerCalibrator calibrator =
754                 new KnownPositionAccelerometerCalibrator(measurements,
755                         true, this);
756 
757         // check default values
758         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
759         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
760         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
761         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
762         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
763         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
764         final Acceleration bx2 = new Acceleration(0.0,
765                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
766         calibrator.getInitialBiasXAsAcceleration(bx2);
767         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
768         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
769         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
770         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
771         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
772         final Acceleration by2 = new Acceleration(0.0,
773                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
774         calibrator.getInitialBiasYAsAcceleration(by2);
775         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
776         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
777         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
778         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
779         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
780         final Acceleration bz2 = new Acceleration(0.0,
781                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
782         calibrator.getInitialBiasZAsAcceleration(bz2);
783         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
784         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
785         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
786         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
787         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
788         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
789         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
790         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
791         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
792         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
793         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
794         final double[] bias1 = calibrator.getInitialBias();
795         assertArrayEquals(bias1, new double[3], 0.0);
796         final double[] bias2 = new double[3];
797         calibrator.getInitialBias(bias2);
798         assertArrayEquals(bias1, bias2, 0.0);
799         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
800         assertEquals(b1, new Matrix(3, 1));
801         final Matrix b2 = new Matrix(3, 1);
802         calibrator.getInitialBiasAsMatrix(b2);
803         assertEquals(b1, b2);
804         final Matrix ma1 = calibrator.getInitialMa();
805         assertEquals(ma1, new Matrix(3, 3));
806         final Matrix ma2 = new Matrix(3, 3);
807         calibrator.getInitialMa(ma2);
808         assertEquals(ma1, ma2);
809         assertNull(calibrator.getEcefPosition());
810         assertNull(calibrator.getNedPosition());
811         assertFalse(calibrator.getNedPosition(null));
812         assertSame(calibrator.getMeasurements(), measurements);
813         assertTrue(calibrator.isCommonAxisUsed());
814         assertSame(calibrator.getListener(), this);
815         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
816         assertFalse(calibrator.isReady());
817         assertFalse(calibrator.isRunning());
818         assertNull(calibrator.getEstimatedBiases());
819         assertFalse(calibrator.getEstimatedBiases(null));
820         assertNull(calibrator.getEstimatedBiasesAsMatrix());
821         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
822         assertNull(calibrator.getEstimatedBiasFx());
823         assertNull(calibrator.getEstimatedBiasFy());
824         assertNull(calibrator.getEstimatedBiasFz());
825         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
826         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
827         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
828         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
829         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
830         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
831         assertNull(calibrator.getEstimatedMa());
832         assertNull(calibrator.getEstimatedSx());
833         assertNull(calibrator.getEstimatedSy());
834         assertNull(calibrator.getEstimatedSz());
835         assertNull(calibrator.getEstimatedMxy());
836         assertNull(calibrator.getEstimatedMxz());
837         assertNull(calibrator.getEstimatedMyx());
838         assertNull(calibrator.getEstimatedMyz());
839         assertNull(calibrator.getEstimatedMzx());
840         assertNull(calibrator.getEstimatedMzy());
841         assertNull(calibrator.getEstimatedCovariance());
842         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
843         assertNull(calibrator.getGroundTruthGravityNorm());
844         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
845         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
846     }
847 
848     @Test
849     public void testConstructor9() throws WrongSizeException {
850         final Matrix ba = generateBa();
851         final double biasX = ba.getElementAtIndex(0);
852         final double biasY = ba.getElementAtIndex(1);
853         final double biasZ = ba.getElementAtIndex(2);
854 
855         final KnownPositionAccelerometerCalibrator calibrator =
856                 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ);
857 
858         // check default values
859         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
860         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
861         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
862         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
863         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
864         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
865         final Acceleration bx2 = new Acceleration(0.0,
866                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
867         calibrator.getInitialBiasXAsAcceleration(bx2);
868         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
869         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
870         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
871         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
872         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
873         final Acceleration by2 = new Acceleration(0.0,
874                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
875         calibrator.getInitialBiasYAsAcceleration(by2);
876         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
877         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
878         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
879         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
880         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
881         final Acceleration bz2 = new Acceleration(0.0,
882                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
883         calibrator.getInitialBiasZAsAcceleration(bz2);
884         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
885         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
886         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
887         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
888         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
889         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
890         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
891         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
892         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
893         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
894         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
895         final double[] bias1 = calibrator.getInitialBias();
896         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
897         final double[] bias2 = new double[3];
898         calibrator.getInitialBias(bias2);
899         assertArrayEquals(bias1, bias2, 0.0);
900         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
901         assertEquals(b1, ba);
902         final Matrix b2 = new Matrix(3, 1);
903         calibrator.getInitialBiasAsMatrix(b2);
904         assertEquals(b1, b2);
905         final Matrix ma1 = calibrator.getInitialMa();
906         assertEquals(ma1, new Matrix(3, 3));
907         final Matrix ma2 = new Matrix(3, 3);
908         calibrator.getInitialMa(ma2);
909         assertEquals(ma1, ma2);
910         assertNull(calibrator.getEcefPosition());
911         assertNull(calibrator.getNedPosition());
912         assertFalse(calibrator.getNedPosition(null));
913         assertNull(calibrator.getMeasurements());
914         assertFalse(calibrator.isCommonAxisUsed());
915         assertNull(calibrator.getListener());
916         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
917         assertFalse(calibrator.isReady());
918         assertFalse(calibrator.isRunning());
919         assertNull(calibrator.getEstimatedBiases());
920         assertFalse(calibrator.getEstimatedBiases(null));
921         assertNull(calibrator.getEstimatedBiasesAsMatrix());
922         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
923         assertNull(calibrator.getEstimatedBiasFx());
924         assertNull(calibrator.getEstimatedBiasFy());
925         assertNull(calibrator.getEstimatedBiasFz());
926         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
927         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
928         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
929         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
930         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
931         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
932         assertNull(calibrator.getEstimatedMa());
933         assertNull(calibrator.getEstimatedSx());
934         assertNull(calibrator.getEstimatedSy());
935         assertNull(calibrator.getEstimatedSz());
936         assertNull(calibrator.getEstimatedMxy());
937         assertNull(calibrator.getEstimatedMxz());
938         assertNull(calibrator.getEstimatedMyx());
939         assertNull(calibrator.getEstimatedMyz());
940         assertNull(calibrator.getEstimatedMzx());
941         assertNull(calibrator.getEstimatedMzy());
942         assertNull(calibrator.getEstimatedCovariance());
943         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
944         assertNull(calibrator.getGroundTruthGravityNorm());
945         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
946         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
947     }
948 
949     @Test
950     public void testConstructor10() throws WrongSizeException {
951         final Matrix ba = generateBa();
952         final double biasX = ba.getElementAtIndex(0);
953         final double biasY = ba.getElementAtIndex(1);
954         final double biasZ = ba.getElementAtIndex(2);
955 
956         final KnownPositionAccelerometerCalibrator calibrator =
957                 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
958                         this);
959 
960         // check default values
961         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
962         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
963         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
964         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
965         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
966         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
967         final Acceleration bx2 = new Acceleration(0.0,
968                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
969         calibrator.getInitialBiasXAsAcceleration(bx2);
970         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
971         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
972         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
973         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
974         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
975         final Acceleration by2 = new Acceleration(0.0,
976                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
977         calibrator.getInitialBiasYAsAcceleration(by2);
978         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
979         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
980         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
981         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
982         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
983         final Acceleration bz2 = new Acceleration(0.0,
984                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
985         calibrator.getInitialBiasZAsAcceleration(bz2);
986         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
987         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
988         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
989         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
990         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
991         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
992         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
993         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
994         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
995         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
996         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
997         final double[] bias1 = calibrator.getInitialBias();
998         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
999         final double[] bias2 = new double[3];
1000         calibrator.getInitialBias(bias2);
1001         assertArrayEquals(bias1, bias2, 0.0);
1002         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1003         assertEquals(b1, ba);
1004         final Matrix b2 = new Matrix(3, 1);
1005         calibrator.getInitialBiasAsMatrix(b2);
1006         assertEquals(b1, b2);
1007         final Matrix ma1 = calibrator.getInitialMa();
1008         assertEquals(ma1, new Matrix(3, 3));
1009         final Matrix ma2 = new Matrix(3, 3);
1010         calibrator.getInitialMa(ma2);
1011         assertEquals(ma1, ma2);
1012         assertNull(calibrator.getEcefPosition());
1013         assertNull(calibrator.getNedPosition());
1014         assertFalse(calibrator.getNedPosition(null));
1015         assertNull(calibrator.getMeasurements());
1016         assertFalse(calibrator.isCommonAxisUsed());
1017         assertSame(calibrator.getListener(), this);
1018         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1019         assertFalse(calibrator.isReady());
1020         assertFalse(calibrator.isRunning());
1021         assertNull(calibrator.getEstimatedBiases());
1022         assertFalse(calibrator.getEstimatedBiases(null));
1023         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1024         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1025         assertNull(calibrator.getEstimatedBiasFx());
1026         assertNull(calibrator.getEstimatedBiasFy());
1027         assertNull(calibrator.getEstimatedBiasFz());
1028         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1029         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1030         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1031         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1032         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1033         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1034         assertNull(calibrator.getEstimatedMa());
1035         assertNull(calibrator.getEstimatedSx());
1036         assertNull(calibrator.getEstimatedSy());
1037         assertNull(calibrator.getEstimatedSz());
1038         assertNull(calibrator.getEstimatedMxy());
1039         assertNull(calibrator.getEstimatedMxz());
1040         assertNull(calibrator.getEstimatedMyx());
1041         assertNull(calibrator.getEstimatedMyz());
1042         assertNull(calibrator.getEstimatedMzx());
1043         assertNull(calibrator.getEstimatedMzy());
1044         assertNull(calibrator.getEstimatedCovariance());
1045         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1046         assertNull(calibrator.getGroundTruthGravityNorm());
1047         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1048         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1049     }
1050 
1051     @Test
1052     public void testConstructor11() throws WrongSizeException {
1053         final Collection<StandardDeviationBodyKinematics> measurements =
1054                 Collections.emptyList();
1055 
1056         final Matrix ba = generateBa();
1057         final double biasX = ba.getElementAtIndex(0);
1058         final double biasY = ba.getElementAtIndex(1);
1059         final double biasZ = ba.getElementAtIndex(2);
1060 
1061         final KnownPositionAccelerometerCalibrator calibrator =
1062                 new KnownPositionAccelerometerCalibrator(measurements,
1063                         biasX, biasY, biasZ);
1064 
1065         // check default values
1066         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1067         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1068         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1069         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1070         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1071         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1072         final Acceleration bx2 = new Acceleration(0.0,
1073                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1074         calibrator.getInitialBiasXAsAcceleration(bx2);
1075         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1076         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1077         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1078         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1079         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1080         final Acceleration by2 = new Acceleration(0.0,
1081                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1082         calibrator.getInitialBiasYAsAcceleration(by2);
1083         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1084         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1085         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1086         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1087         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1088         final Acceleration bz2 = new Acceleration(0.0,
1089                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1090         calibrator.getInitialBiasZAsAcceleration(bz2);
1091         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1092         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1093         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1094         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1095         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1096         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1097         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1098         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1099         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1100         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1101         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1102         final double[] bias1 = calibrator.getInitialBias();
1103         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1104         final double[] bias2 = new double[3];
1105         calibrator.getInitialBias(bias2);
1106         assertArrayEquals(bias1, bias2, 0.0);
1107         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1108         assertEquals(b1, ba);
1109         final Matrix b2 = new Matrix(3, 1);
1110         calibrator.getInitialBiasAsMatrix(b2);
1111         assertEquals(b1, b2);
1112         final Matrix ma1 = calibrator.getInitialMa();
1113         assertEquals(ma1, new Matrix(3, 3));
1114         final Matrix ma2 = new Matrix(3, 3);
1115         calibrator.getInitialMa(ma2);
1116         assertEquals(ma1, ma2);
1117         assertNull(calibrator.getEcefPosition());
1118         assertNull(calibrator.getNedPosition());
1119         assertFalse(calibrator.getNedPosition(null));
1120         assertSame(calibrator.getMeasurements(), measurements);
1121         assertFalse(calibrator.isCommonAxisUsed());
1122         assertNull(calibrator.getListener());
1123         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1124         assertFalse(calibrator.isReady());
1125         assertFalse(calibrator.isRunning());
1126         assertNull(calibrator.getEstimatedBiases());
1127         assertFalse(calibrator.getEstimatedBiases(null));
1128         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1129         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1130         assertNull(calibrator.getEstimatedBiasFx());
1131         assertNull(calibrator.getEstimatedBiasFy());
1132         assertNull(calibrator.getEstimatedBiasFz());
1133         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1134         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1135         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1136         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1137         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1138         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1139         assertNull(calibrator.getEstimatedMa());
1140         assertNull(calibrator.getEstimatedSx());
1141         assertNull(calibrator.getEstimatedSy());
1142         assertNull(calibrator.getEstimatedSz());
1143         assertNull(calibrator.getEstimatedMxy());
1144         assertNull(calibrator.getEstimatedMxz());
1145         assertNull(calibrator.getEstimatedMyx());
1146         assertNull(calibrator.getEstimatedMyz());
1147         assertNull(calibrator.getEstimatedMzx());
1148         assertNull(calibrator.getEstimatedMzy());
1149         assertNull(calibrator.getEstimatedCovariance());
1150         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1151         assertNull(calibrator.getGroundTruthGravityNorm());
1152         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1153         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1154     }
1155 
1156     @Test
1157     public void testConstructor12() throws WrongSizeException {
1158         final Collection<StandardDeviationBodyKinematics> measurements =
1159                 Collections.emptyList();
1160 
1161         final Matrix ba = generateBa();
1162         final double biasX = ba.getElementAtIndex(0);
1163         final double biasY = ba.getElementAtIndex(1);
1164         final double biasZ = ba.getElementAtIndex(2);
1165 
1166         final KnownPositionAccelerometerCalibrator calibrator =
1167                 new KnownPositionAccelerometerCalibrator(measurements,
1168                         biasX, biasY, biasZ, this);
1169 
1170         // check default values
1171         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1172         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1173         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1174         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1175         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1176         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1177         final Acceleration bx2 = new Acceleration(0.0,
1178                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1179         calibrator.getInitialBiasXAsAcceleration(bx2);
1180         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1181         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1182         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1183         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1184         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1185         final Acceleration by2 = new Acceleration(0.0,
1186                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1187         calibrator.getInitialBiasYAsAcceleration(by2);
1188         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1189         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1190         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1191         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1192         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1193         final Acceleration bz2 = new Acceleration(0.0,
1194                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1195         calibrator.getInitialBiasZAsAcceleration(bz2);
1196         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1197         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1198         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1199         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1200         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1201         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1202         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1203         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1204         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1205         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1206         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1207         final double[] bias1 = calibrator.getInitialBias();
1208         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1209         final double[] bias2 = new double[3];
1210         calibrator.getInitialBias(bias2);
1211         assertArrayEquals(bias1, bias2, 0.0);
1212         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1213         assertEquals(b1, ba);
1214         final Matrix b2 = new Matrix(3, 1);
1215         calibrator.getInitialBiasAsMatrix(b2);
1216         assertEquals(b1, b2);
1217         final Matrix ma1 = calibrator.getInitialMa();
1218         assertEquals(ma1, new Matrix(3, 3));
1219         final Matrix ma2 = new Matrix(3, 3);
1220         calibrator.getInitialMa(ma2);
1221         assertEquals(ma1, ma2);
1222         assertNull(calibrator.getEcefPosition());
1223         assertNull(calibrator.getNedPosition());
1224         assertFalse(calibrator.getNedPosition(null));
1225         assertSame(calibrator.getMeasurements(), measurements);
1226         assertFalse(calibrator.isCommonAxisUsed());
1227         assertSame(calibrator.getListener(), this);
1228         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1229         assertFalse(calibrator.isReady());
1230         assertFalse(calibrator.isRunning());
1231         assertNull(calibrator.getEstimatedBiases());
1232         assertFalse(calibrator.getEstimatedBiases(null));
1233         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1234         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1235         assertNull(calibrator.getEstimatedBiasFx());
1236         assertNull(calibrator.getEstimatedBiasFy());
1237         assertNull(calibrator.getEstimatedBiasFz());
1238         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1239         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1240         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1241         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1242         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1243         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1244         assertNull(calibrator.getEstimatedMa());
1245         assertNull(calibrator.getEstimatedSx());
1246         assertNull(calibrator.getEstimatedSy());
1247         assertNull(calibrator.getEstimatedSz());
1248         assertNull(calibrator.getEstimatedMxy());
1249         assertNull(calibrator.getEstimatedMxz());
1250         assertNull(calibrator.getEstimatedMyx());
1251         assertNull(calibrator.getEstimatedMyz());
1252         assertNull(calibrator.getEstimatedMzx());
1253         assertNull(calibrator.getEstimatedMzy());
1254         assertNull(calibrator.getEstimatedCovariance());
1255         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1256         assertNull(calibrator.getGroundTruthGravityNorm());
1257         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1258         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1259     }
1260 
1261     @Test
1262     public void testConstructor13() throws WrongSizeException {
1263         final Matrix ba = generateBa();
1264         final double biasX = ba.getElementAtIndex(0);
1265         final double biasY = ba.getElementAtIndex(1);
1266         final double biasZ = ba.getElementAtIndex(2);
1267 
1268         final KnownPositionAccelerometerCalibrator calibrator =
1269                 new KnownPositionAccelerometerCalibrator(true,
1270                         biasX, biasY, biasZ);
1271 
1272         // check default values
1273         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1274         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1275         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1276         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1277         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1278         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1279         final Acceleration bx2 = new Acceleration(0.0,
1280                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1281         calibrator.getInitialBiasXAsAcceleration(bx2);
1282         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1283         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1284         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1285         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1286         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1287         final Acceleration by2 = new Acceleration(0.0,
1288                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1289         calibrator.getInitialBiasYAsAcceleration(by2);
1290         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1291         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1292         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1293         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1294         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1295         final Acceleration bz2 = new Acceleration(0.0,
1296                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1297         calibrator.getInitialBiasZAsAcceleration(bz2);
1298         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1299         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1300         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1301         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1302         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1303         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1304         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1305         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1306         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1307         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1308         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1309         final double[] bias1 = calibrator.getInitialBias();
1310         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1311         final double[] bias2 = new double[3];
1312         calibrator.getInitialBias(bias2);
1313         assertArrayEquals(bias1, bias2, 0.0);
1314         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1315         assertEquals(b1, ba);
1316         final Matrix b2 = new Matrix(3, 1);
1317         calibrator.getInitialBiasAsMatrix(b2);
1318         assertEquals(b1, b2);
1319         final Matrix ma1 = calibrator.getInitialMa();
1320         assertEquals(ma1, new Matrix(3, 3));
1321         final Matrix ma2 = new Matrix(3, 3);
1322         calibrator.getInitialMa(ma2);
1323         assertEquals(ma1, ma2);
1324         assertNull(calibrator.getEcefPosition());
1325         assertNull(calibrator.getNedPosition());
1326         assertFalse(calibrator.getNedPosition(null));
1327         assertNull(calibrator.getMeasurements());
1328         assertTrue(calibrator.isCommonAxisUsed());
1329         assertNull(calibrator.getListener());
1330         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1331         assertFalse(calibrator.isReady());
1332         assertFalse(calibrator.isRunning());
1333         assertNull(calibrator.getEstimatedBiases());
1334         assertFalse(calibrator.getEstimatedBiases(null));
1335         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1336         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1337         assertNull(calibrator.getEstimatedBiasFx());
1338         assertNull(calibrator.getEstimatedBiasFy());
1339         assertNull(calibrator.getEstimatedBiasFz());
1340         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1341         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1342         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1343         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1344         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1345         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1346         assertNull(calibrator.getEstimatedMa());
1347         assertNull(calibrator.getEstimatedSx());
1348         assertNull(calibrator.getEstimatedSy());
1349         assertNull(calibrator.getEstimatedSz());
1350         assertNull(calibrator.getEstimatedMxy());
1351         assertNull(calibrator.getEstimatedMxz());
1352         assertNull(calibrator.getEstimatedMyx());
1353         assertNull(calibrator.getEstimatedMyz());
1354         assertNull(calibrator.getEstimatedMzx());
1355         assertNull(calibrator.getEstimatedMzy());
1356         assertNull(calibrator.getEstimatedCovariance());
1357         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1358         assertNull(calibrator.getGroundTruthGravityNorm());
1359         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1360         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1361     }
1362 
1363     @Test
1364     public void testConstructor14() throws WrongSizeException {
1365         final Matrix ba = generateBa();
1366         final double biasX = ba.getElementAtIndex(0);
1367         final double biasY = ba.getElementAtIndex(1);
1368         final double biasZ = ba.getElementAtIndex(2);
1369 
1370         final KnownPositionAccelerometerCalibrator calibrator =
1371                 new KnownPositionAccelerometerCalibrator(true,
1372                         biasX, biasY, biasZ, this);
1373 
1374         // check default values
1375         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1376         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1377         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1378         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1379         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1380         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1381         final Acceleration bx2 = new Acceleration(0.0,
1382                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1383         calibrator.getInitialBiasXAsAcceleration(bx2);
1384         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1385         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1386         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1387         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1388         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1389         final Acceleration by2 = new Acceleration(0.0,
1390                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1391         calibrator.getInitialBiasYAsAcceleration(by2);
1392         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1393         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1394         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1395         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1396         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1397         final Acceleration bz2 = new Acceleration(0.0,
1398                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1399         calibrator.getInitialBiasZAsAcceleration(bz2);
1400         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1401         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1402         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1403         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1404         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1405         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1406         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1407         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1408         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1409         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1410         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1411         final double[] bias1 = calibrator.getInitialBias();
1412         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1413         final double[] bias2 = new double[3];
1414         calibrator.getInitialBias(bias2);
1415         assertArrayEquals(bias1, bias2, 0.0);
1416         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1417         assertEquals(b1, ba);
1418         final Matrix b2 = new Matrix(3, 1);
1419         calibrator.getInitialBiasAsMatrix(b2);
1420         assertEquals(b1, b2);
1421         final Matrix ma1 = calibrator.getInitialMa();
1422         assertEquals(ma1, new Matrix(3, 3));
1423         final Matrix ma2 = new Matrix(3, 3);
1424         calibrator.getInitialMa(ma2);
1425         assertEquals(ma1, ma2);
1426         assertNull(calibrator.getEcefPosition());
1427         assertNull(calibrator.getNedPosition());
1428         assertFalse(calibrator.getNedPosition(null));
1429         assertNull(calibrator.getMeasurements());
1430         assertTrue(calibrator.isCommonAxisUsed());
1431         assertSame(calibrator.getListener(), this);
1432         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1433         assertFalse(calibrator.isReady());
1434         assertFalse(calibrator.isRunning());
1435         assertNull(calibrator.getEstimatedBiases());
1436         assertFalse(calibrator.getEstimatedBiases(null));
1437         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1438         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1439         assertNull(calibrator.getEstimatedBiasFx());
1440         assertNull(calibrator.getEstimatedBiasFy());
1441         assertNull(calibrator.getEstimatedBiasFz());
1442         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1443         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1444         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1445         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1446         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1447         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1448         assertNull(calibrator.getEstimatedMa());
1449         assertNull(calibrator.getEstimatedSx());
1450         assertNull(calibrator.getEstimatedSy());
1451         assertNull(calibrator.getEstimatedSz());
1452         assertNull(calibrator.getEstimatedMxy());
1453         assertNull(calibrator.getEstimatedMxz());
1454         assertNull(calibrator.getEstimatedMyx());
1455         assertNull(calibrator.getEstimatedMyz());
1456         assertNull(calibrator.getEstimatedMzx());
1457         assertNull(calibrator.getEstimatedMzy());
1458         assertNull(calibrator.getEstimatedCovariance());
1459         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1460         assertNull(calibrator.getGroundTruthGravityNorm());
1461         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1462         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1463     }
1464 
1465     @Test
1466     public void testConstructor15() throws WrongSizeException {
1467         final Collection<StandardDeviationBodyKinematics> measurements =
1468                 Collections.emptyList();
1469 
1470         final Matrix ba = generateBa();
1471         final double biasX = ba.getElementAtIndex(0);
1472         final double biasY = ba.getElementAtIndex(1);
1473         final double biasZ = ba.getElementAtIndex(2);
1474 
1475         final KnownPositionAccelerometerCalibrator calibrator =
1476                 new KnownPositionAccelerometerCalibrator(measurements,
1477                         true, biasX, biasY, biasZ);
1478 
1479         // check default values
1480         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1481         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1482         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1483         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1484         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1485         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1486         final Acceleration bx2 = new Acceleration(0.0,
1487                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1488         calibrator.getInitialBiasXAsAcceleration(bx2);
1489         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1490         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1491         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1492         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1493         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1494         final Acceleration by2 = new Acceleration(0.0,
1495                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1496         calibrator.getInitialBiasYAsAcceleration(by2);
1497         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1498         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1499         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1500         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1501         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1502         final Acceleration bz2 = new Acceleration(0.0,
1503                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1504         calibrator.getInitialBiasZAsAcceleration(bz2);
1505         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1506         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1507         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1508         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1509         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1510         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1511         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1512         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1513         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1514         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1515         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1516         final double[] bias1 = calibrator.getInitialBias();
1517         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1518         final double[] bias2 = new double[3];
1519         calibrator.getInitialBias(bias2);
1520         assertArrayEquals(bias1, bias2, 0.0);
1521         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1522         assertEquals(b1, ba);
1523         final Matrix b2 = new Matrix(3, 1);
1524         calibrator.getInitialBiasAsMatrix(b2);
1525         assertEquals(b1, b2);
1526         final Matrix ma1 = calibrator.getInitialMa();
1527         assertEquals(ma1, new Matrix(3, 3));
1528         final Matrix ma2 = new Matrix(3, 3);
1529         calibrator.getInitialMa(ma2);
1530         assertEquals(ma1, ma2);
1531         assertNull(calibrator.getEcefPosition());
1532         assertNull(calibrator.getNedPosition());
1533         assertFalse(calibrator.getNedPosition(null));
1534         assertSame(calibrator.getMeasurements(), measurements);
1535         assertTrue(calibrator.isCommonAxisUsed());
1536         assertNull(calibrator.getListener());
1537         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1538         assertFalse(calibrator.isReady());
1539         assertFalse(calibrator.isRunning());
1540         assertNull(calibrator.getEstimatedBiases());
1541         assertFalse(calibrator.getEstimatedBiases(null));
1542         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1543         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1544         assertNull(calibrator.getEstimatedBiasFx());
1545         assertNull(calibrator.getEstimatedBiasFy());
1546         assertNull(calibrator.getEstimatedBiasFz());
1547         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1548         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1549         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1550         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1551         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1552         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1553         assertNull(calibrator.getEstimatedMa());
1554         assertNull(calibrator.getEstimatedSx());
1555         assertNull(calibrator.getEstimatedSy());
1556         assertNull(calibrator.getEstimatedSz());
1557         assertNull(calibrator.getEstimatedMxy());
1558         assertNull(calibrator.getEstimatedMxz());
1559         assertNull(calibrator.getEstimatedMyx());
1560         assertNull(calibrator.getEstimatedMyz());
1561         assertNull(calibrator.getEstimatedMzx());
1562         assertNull(calibrator.getEstimatedMzy());
1563         assertNull(calibrator.getEstimatedCovariance());
1564         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1565         assertNull(calibrator.getGroundTruthGravityNorm());
1566         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1567         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1568     }
1569 
1570     @Test
1571     public void testConstructor16() throws WrongSizeException {
1572         final Collection<StandardDeviationBodyKinematics> measurements =
1573                 Collections.emptyList();
1574 
1575         final Matrix ba = generateBa();
1576         final double biasX = ba.getElementAtIndex(0);
1577         final double biasY = ba.getElementAtIndex(1);
1578         final double biasZ = ba.getElementAtIndex(2);
1579 
1580         final KnownPositionAccelerometerCalibrator calibrator =
1581                 new KnownPositionAccelerometerCalibrator(measurements,
1582                         true, biasX, biasY, biasZ, this);
1583 
1584         // check default values
1585         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1586         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1587         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1588         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1589         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1590         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1591         final Acceleration bx2 = new Acceleration(0.0,
1592                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1593         calibrator.getInitialBiasXAsAcceleration(bx2);
1594         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1595         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1596         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1597         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1598         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1599         final Acceleration by2 = new Acceleration(0.0,
1600                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1601         calibrator.getInitialBiasYAsAcceleration(by2);
1602         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1603         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1604         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1605         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1606         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1607         final Acceleration bz2 = new Acceleration(0.0,
1608                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1609         calibrator.getInitialBiasZAsAcceleration(bz2);
1610         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1611         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1612         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1613         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1614         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1615         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1616         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1617         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1618         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1619         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1620         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1621         final double[] bias1 = calibrator.getInitialBias();
1622         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1623         final double[] bias2 = new double[3];
1624         calibrator.getInitialBias(bias2);
1625         assertArrayEquals(bias1, bias2, 0.0);
1626         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1627         assertEquals(b1, ba);
1628         final Matrix b2 = new Matrix(3, 1);
1629         calibrator.getInitialBiasAsMatrix(b2);
1630         assertEquals(b1, b2);
1631         final Matrix ma1 = calibrator.getInitialMa();
1632         assertEquals(ma1, new Matrix(3, 3));
1633         final Matrix ma2 = new Matrix(3, 3);
1634         calibrator.getInitialMa(ma2);
1635         assertEquals(ma1, ma2);
1636         assertNull(calibrator.getEcefPosition());
1637         assertNull(calibrator.getNedPosition());
1638         assertFalse(calibrator.getNedPosition(null));
1639         assertSame(calibrator.getMeasurements(), measurements);
1640         assertTrue(calibrator.isCommonAxisUsed());
1641         assertSame(calibrator.getListener(), this);
1642         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1643         assertFalse(calibrator.isReady());
1644         assertFalse(calibrator.isRunning());
1645         assertNull(calibrator.getEstimatedBiases());
1646         assertFalse(calibrator.getEstimatedBiases(null));
1647         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1648         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1649         assertNull(calibrator.getEstimatedBiasFx());
1650         assertNull(calibrator.getEstimatedBiasFy());
1651         assertNull(calibrator.getEstimatedBiasFz());
1652         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1653         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1654         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1655         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1656         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1657         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1658         assertNull(calibrator.getEstimatedMa());
1659         assertNull(calibrator.getEstimatedSx());
1660         assertNull(calibrator.getEstimatedSy());
1661         assertNull(calibrator.getEstimatedSz());
1662         assertNull(calibrator.getEstimatedMxy());
1663         assertNull(calibrator.getEstimatedMxz());
1664         assertNull(calibrator.getEstimatedMyx());
1665         assertNull(calibrator.getEstimatedMyz());
1666         assertNull(calibrator.getEstimatedMzx());
1667         assertNull(calibrator.getEstimatedMzy());
1668         assertNull(calibrator.getEstimatedCovariance());
1669         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1670         assertNull(calibrator.getGroundTruthGravityNorm());
1671         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1672         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1673     }
1674 
1675     @Test
1676     public void testConstructor17() throws WrongSizeException {
1677         final Matrix ba = generateBa();
1678         final double biasX = ba.getElementAtIndex(0);
1679         final double biasY = ba.getElementAtIndex(1);
1680         final double biasZ = ba.getElementAtIndex(2);
1681 
1682         final Acceleration bx = new Acceleration(biasX,
1683                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1684         final Acceleration by = new Acceleration(biasY,
1685                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1686         final Acceleration bz = new Acceleration(biasZ,
1687                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1688 
1689         final KnownPositionAccelerometerCalibrator calibrator =
1690                 new KnownPositionAccelerometerCalibrator(bx, by, bz);
1691 
1692         // check default values
1693         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1694         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1695         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1696         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1697         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1698         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1699         final Acceleration bx2 = new Acceleration(0.0,
1700                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1701         calibrator.getInitialBiasXAsAcceleration(bx2);
1702         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1703         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1704         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1705         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1706         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1707         final Acceleration by2 = new Acceleration(0.0,
1708                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1709         calibrator.getInitialBiasYAsAcceleration(by2);
1710         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1711         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1712         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1713         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1714         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1715         final Acceleration bz2 = new Acceleration(0.0,
1716                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1717         calibrator.getInitialBiasZAsAcceleration(bz2);
1718         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1719         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1720         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1721         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1722         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1723         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1724         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1725         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1726         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1727         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1728         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1729         final double[] bias1 = calibrator.getInitialBias();
1730         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1731         final double[] bias2 = new double[3];
1732         calibrator.getInitialBias(bias2);
1733         assertArrayEquals(bias1, bias2, 0.0);
1734         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1735         assertEquals(b1, ba);
1736         final Matrix b2 = new Matrix(3, 1);
1737         calibrator.getInitialBiasAsMatrix(b2);
1738         assertEquals(b1, b2);
1739         final Matrix ma1 = calibrator.getInitialMa();
1740         assertEquals(ma1, new Matrix(3, 3));
1741         final Matrix ma2 = new Matrix(3, 3);
1742         calibrator.getInitialMa(ma2);
1743         assertEquals(ma1, ma2);
1744         assertNull(calibrator.getEcefPosition());
1745         assertNull(calibrator.getNedPosition());
1746         assertFalse(calibrator.getNedPosition(null));
1747         assertNull(calibrator.getMeasurements());
1748         assertFalse(calibrator.isCommonAxisUsed());
1749         assertNull(calibrator.getListener());
1750         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1751         assertFalse(calibrator.isReady());
1752         assertFalse(calibrator.isRunning());
1753         assertNull(calibrator.getEstimatedBiases());
1754         assertFalse(calibrator.getEstimatedBiases(null));
1755         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1756         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1757         assertNull(calibrator.getEstimatedBiasFx());
1758         assertNull(calibrator.getEstimatedBiasFy());
1759         assertNull(calibrator.getEstimatedBiasFz());
1760         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1761         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1762         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1763         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1764         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1765         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1766         assertNull(calibrator.getEstimatedMa());
1767         assertNull(calibrator.getEstimatedSx());
1768         assertNull(calibrator.getEstimatedSy());
1769         assertNull(calibrator.getEstimatedSz());
1770         assertNull(calibrator.getEstimatedMxy());
1771         assertNull(calibrator.getEstimatedMxz());
1772         assertNull(calibrator.getEstimatedMyx());
1773         assertNull(calibrator.getEstimatedMyz());
1774         assertNull(calibrator.getEstimatedMzx());
1775         assertNull(calibrator.getEstimatedMzy());
1776         assertNull(calibrator.getEstimatedCovariance());
1777         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1778         assertNull(calibrator.getGroundTruthGravityNorm());
1779         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1780         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1781     }
1782 
1783     @Test
1784     public void testConstructor18() throws WrongSizeException {
1785         final Matrix ba = generateBa();
1786         final double biasX = ba.getElementAtIndex(0);
1787         final double biasY = ba.getElementAtIndex(1);
1788         final double biasZ = ba.getElementAtIndex(2);
1789 
1790         final Acceleration bx = new Acceleration(biasX,
1791                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1792         final Acceleration by = new Acceleration(biasY,
1793                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1794         final Acceleration bz = new Acceleration(biasZ,
1795                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1796 
1797         final KnownPositionAccelerometerCalibrator calibrator =
1798                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
1799                         this);
1800 
1801         // check default values
1802         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1803         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1804         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1805         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1806         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1807         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1808         final Acceleration bx2 = new Acceleration(0.0,
1809                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1810         calibrator.getInitialBiasXAsAcceleration(bx2);
1811         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1812         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1813         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1814         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1815         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1816         final Acceleration by2 = new Acceleration(0.0,
1817                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1818         calibrator.getInitialBiasYAsAcceleration(by2);
1819         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1820         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1821         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1822         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1823         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1824         final Acceleration bz2 = new Acceleration(0.0,
1825                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1826         calibrator.getInitialBiasZAsAcceleration(bz2);
1827         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1828         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1829         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1830         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1831         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1832         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1833         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1834         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1835         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1836         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1837         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1838         final double[] bias1 = calibrator.getInitialBias();
1839         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1840         final double[] bias2 = new double[3];
1841         calibrator.getInitialBias(bias2);
1842         assertArrayEquals(bias1, bias2, 0.0);
1843         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1844         assertEquals(b1, ba);
1845         final Matrix b2 = new Matrix(3, 1);
1846         calibrator.getInitialBiasAsMatrix(b2);
1847         assertEquals(b1, b2);
1848         final Matrix ma1 = calibrator.getInitialMa();
1849         assertEquals(ma1, new Matrix(3, 3));
1850         final Matrix ma2 = new Matrix(3, 3);
1851         calibrator.getInitialMa(ma2);
1852         assertEquals(ma1, ma2);
1853         assertNull(calibrator.getEcefPosition());
1854         assertNull(calibrator.getNedPosition());
1855         assertFalse(calibrator.getNedPosition(null));
1856         assertNull(calibrator.getMeasurements());
1857         assertFalse(calibrator.isCommonAxisUsed());
1858         assertSame(calibrator.getListener(), this);
1859         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1860         assertFalse(calibrator.isReady());
1861         assertFalse(calibrator.isRunning());
1862         assertNull(calibrator.getEstimatedBiases());
1863         assertFalse(calibrator.getEstimatedBiases(null));
1864         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1865         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1866         assertNull(calibrator.getEstimatedBiasFx());
1867         assertNull(calibrator.getEstimatedBiasFy());
1868         assertNull(calibrator.getEstimatedBiasFz());
1869         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1870         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1871         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1872         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1873         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1874         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1875         assertNull(calibrator.getEstimatedMa());
1876         assertNull(calibrator.getEstimatedSx());
1877         assertNull(calibrator.getEstimatedSy());
1878         assertNull(calibrator.getEstimatedSz());
1879         assertNull(calibrator.getEstimatedMxy());
1880         assertNull(calibrator.getEstimatedMxz());
1881         assertNull(calibrator.getEstimatedMyx());
1882         assertNull(calibrator.getEstimatedMyz());
1883         assertNull(calibrator.getEstimatedMzx());
1884         assertNull(calibrator.getEstimatedMzy());
1885         assertNull(calibrator.getEstimatedCovariance());
1886         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1887         assertNull(calibrator.getGroundTruthGravityNorm());
1888         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1889         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1890     }
1891 
1892     @Test
1893     public void testConstructor19() throws WrongSizeException {
1894         final Collection<StandardDeviationBodyKinematics> measurements =
1895                 Collections.emptyList();
1896 
1897         final Matrix ba = generateBa();
1898         final double biasX = ba.getElementAtIndex(0);
1899         final double biasY = ba.getElementAtIndex(1);
1900         final double biasZ = ba.getElementAtIndex(2);
1901 
1902         final Acceleration bx = new Acceleration(biasX,
1903                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1904         final Acceleration by = new Acceleration(biasY,
1905                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1906         final Acceleration bz = new Acceleration(biasZ,
1907                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1908 
1909         final KnownPositionAccelerometerCalibrator calibrator =
1910                 new KnownPositionAccelerometerCalibrator(measurements,
1911                         bx, by, bz);
1912 
1913         // check default values
1914         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1915         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1916         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1917         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1918         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1919         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1920         final Acceleration bx2 = new Acceleration(0.0,
1921                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1922         calibrator.getInitialBiasXAsAcceleration(bx2);
1923         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1924         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1925         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1926         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1927         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1928         final Acceleration by2 = new Acceleration(0.0,
1929                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1930         calibrator.getInitialBiasYAsAcceleration(by2);
1931         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1932         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1933         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1934         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1935         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1936         final Acceleration bz2 = new Acceleration(0.0,
1937                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1938         calibrator.getInitialBiasZAsAcceleration(bz2);
1939         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1940         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1941         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1942         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1943         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1944         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1945         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1946         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1947         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1948         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1949         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1950         final double[] bias1 = calibrator.getInitialBias();
1951         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1952         final double[] bias2 = new double[3];
1953         calibrator.getInitialBias(bias2);
1954         assertArrayEquals(bias1, bias2, 0.0);
1955         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1956         assertEquals(b1, ba);
1957         final Matrix b2 = new Matrix(3, 1);
1958         calibrator.getInitialBiasAsMatrix(b2);
1959         assertEquals(b1, b2);
1960         final Matrix ma1 = calibrator.getInitialMa();
1961         assertEquals(ma1, new Matrix(3, 3));
1962         final Matrix ma2 = new Matrix(3, 3);
1963         calibrator.getInitialMa(ma2);
1964         assertEquals(ma1, ma2);
1965         assertNull(calibrator.getEcefPosition());
1966         assertNull(calibrator.getNedPosition());
1967         assertFalse(calibrator.getNedPosition(null));
1968         assertSame(calibrator.getMeasurements(), measurements);
1969         assertFalse(calibrator.isCommonAxisUsed());
1970         assertNull(calibrator.getListener());
1971         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1972         assertFalse(calibrator.isReady());
1973         assertFalse(calibrator.isRunning());
1974         assertNull(calibrator.getEstimatedBiases());
1975         assertFalse(calibrator.getEstimatedBiases(null));
1976         assertNull(calibrator.getEstimatedBiasesAsMatrix());
1977         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1978         assertNull(calibrator.getEstimatedBiasFx());
1979         assertNull(calibrator.getEstimatedBiasFy());
1980         assertNull(calibrator.getEstimatedBiasFz());
1981         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1982         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1983         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1984         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1985         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1986         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1987         assertNull(calibrator.getEstimatedMa());
1988         assertNull(calibrator.getEstimatedSx());
1989         assertNull(calibrator.getEstimatedSy());
1990         assertNull(calibrator.getEstimatedSz());
1991         assertNull(calibrator.getEstimatedMxy());
1992         assertNull(calibrator.getEstimatedMxz());
1993         assertNull(calibrator.getEstimatedMyx());
1994         assertNull(calibrator.getEstimatedMyz());
1995         assertNull(calibrator.getEstimatedMzx());
1996         assertNull(calibrator.getEstimatedMzy());
1997         assertNull(calibrator.getEstimatedCovariance());
1998         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1999         assertNull(calibrator.getGroundTruthGravityNorm());
2000         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2001         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2002     }
2003 
2004     @Test
2005     public void testConstructor20() throws WrongSizeException {
2006         final Collection<StandardDeviationBodyKinematics> measurements =
2007                 Collections.emptyList();
2008 
2009         final Matrix ba = generateBa();
2010         final double biasX = ba.getElementAtIndex(0);
2011         final double biasY = ba.getElementAtIndex(1);
2012         final double biasZ = ba.getElementAtIndex(2);
2013 
2014         final Acceleration bx = new Acceleration(biasX,
2015                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2016         final Acceleration by = new Acceleration(biasY,
2017                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2018         final Acceleration bz = new Acceleration(biasZ,
2019                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2020 
2021         final KnownPositionAccelerometerCalibrator calibrator =
2022                 new KnownPositionAccelerometerCalibrator(measurements,
2023                         bx, by, bz, this);
2024 
2025         // check default values
2026         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2027         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2028         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2029         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2030         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2031         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2032         final Acceleration bx2 = new Acceleration(0.0,
2033                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2034         calibrator.getInitialBiasXAsAcceleration(bx2);
2035         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2036         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2037         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2038         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2039         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2040         final Acceleration by2 = new Acceleration(0.0,
2041                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2042         calibrator.getInitialBiasYAsAcceleration(by2);
2043         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2044         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2045         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2046         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2047         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2048         final Acceleration bz2 = new Acceleration(0.0,
2049                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2050         calibrator.getInitialBiasZAsAcceleration(bz2);
2051         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2052         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2053         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2054         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2055         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2056         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2057         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2058         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2059         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2060         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2061         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2062         final double[] bias1 = calibrator.getInitialBias();
2063         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2064         final double[] bias2 = new double[3];
2065         calibrator.getInitialBias(bias2);
2066         assertArrayEquals(bias1, bias2, 0.0);
2067         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2068         assertEquals(b1, ba);
2069         final Matrix b2 = new Matrix(3, 1);
2070         calibrator.getInitialBiasAsMatrix(b2);
2071         assertEquals(b1, b2);
2072         final Matrix ma1 = calibrator.getInitialMa();
2073         assertEquals(ma1, new Matrix(3, 3));
2074         final Matrix ma2 = new Matrix(3, 3);
2075         calibrator.getInitialMa(ma2);
2076         assertEquals(ma1, ma2);
2077         assertNull(calibrator.getEcefPosition());
2078         assertNull(calibrator.getNedPosition());
2079         assertFalse(calibrator.getNedPosition(null));
2080         assertSame(calibrator.getMeasurements(), measurements);
2081         assertFalse(calibrator.isCommonAxisUsed());
2082         assertSame(calibrator.getListener(), this);
2083         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2084         assertFalse(calibrator.isReady());
2085         assertFalse(calibrator.isRunning());
2086         assertNull(calibrator.getEstimatedBiases());
2087         assertFalse(calibrator.getEstimatedBiases(null));
2088         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2089         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2090         assertNull(calibrator.getEstimatedBiasFx());
2091         assertNull(calibrator.getEstimatedBiasFy());
2092         assertNull(calibrator.getEstimatedBiasFz());
2093         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2094         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2095         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2096         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2097         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2098         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2099         assertNull(calibrator.getEstimatedMa());
2100         assertNull(calibrator.getEstimatedSx());
2101         assertNull(calibrator.getEstimatedSy());
2102         assertNull(calibrator.getEstimatedSz());
2103         assertNull(calibrator.getEstimatedMxy());
2104         assertNull(calibrator.getEstimatedMxz());
2105         assertNull(calibrator.getEstimatedMyx());
2106         assertNull(calibrator.getEstimatedMyz());
2107         assertNull(calibrator.getEstimatedMzx());
2108         assertNull(calibrator.getEstimatedMzy());
2109         assertNull(calibrator.getEstimatedCovariance());
2110         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2111         assertNull(calibrator.getGroundTruthGravityNorm());
2112         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2113         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2114     }
2115 
2116     @Test
2117     public void testConstructor21() throws WrongSizeException {
2118         final Matrix ba = generateBa();
2119         final double biasX = ba.getElementAtIndex(0);
2120         final double biasY = ba.getElementAtIndex(1);
2121         final double biasZ = ba.getElementAtIndex(2);
2122 
2123         final Acceleration bx = new Acceleration(biasX,
2124                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2125         final Acceleration by = new Acceleration(biasY,
2126                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2127         final Acceleration bz = new Acceleration(biasZ,
2128                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2129 
2130         final KnownPositionAccelerometerCalibrator calibrator =
2131                 new KnownPositionAccelerometerCalibrator(true,
2132                         bx, by, bz);
2133 
2134         // check default values
2135         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2136         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2137         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2138         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2139         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2140         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2141         final Acceleration bx2 = new Acceleration(0.0,
2142                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2143         calibrator.getInitialBiasXAsAcceleration(bx2);
2144         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2145         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2146         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2147         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2148         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2149         final Acceleration by2 = new Acceleration(0.0,
2150                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2151         calibrator.getInitialBiasYAsAcceleration(by2);
2152         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2153         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2154         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2155         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2156         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2157         final Acceleration bz2 = new Acceleration(0.0,
2158                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2159         calibrator.getInitialBiasZAsAcceleration(bz2);
2160         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2161         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2162         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2163         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2164         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2165         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2166         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2167         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2168         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2169         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2170         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2171         final double[] bias1 = calibrator.getInitialBias();
2172         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2173         final double[] bias2 = new double[3];
2174         calibrator.getInitialBias(bias2);
2175         assertArrayEquals(bias1, bias2, 0.0);
2176         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2177         assertEquals(b1, ba);
2178         final Matrix b2 = new Matrix(3, 1);
2179         calibrator.getInitialBiasAsMatrix(b2);
2180         assertEquals(b1, b2);
2181         final Matrix ma1 = calibrator.getInitialMa();
2182         assertEquals(ma1, new Matrix(3, 3));
2183         final Matrix ma2 = new Matrix(3, 3);
2184         calibrator.getInitialMa(ma2);
2185         assertEquals(ma1, ma2);
2186         assertNull(calibrator.getEcefPosition());
2187         assertNull(calibrator.getNedPosition());
2188         assertFalse(calibrator.getNedPosition(null));
2189         assertNull(calibrator.getMeasurements());
2190         assertTrue(calibrator.isCommonAxisUsed());
2191         assertNull(calibrator.getListener());
2192         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2193         assertFalse(calibrator.isReady());
2194         assertFalse(calibrator.isRunning());
2195         assertNull(calibrator.getEstimatedBiases());
2196         assertFalse(calibrator.getEstimatedBiases(null));
2197         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2198         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2199         assertNull(calibrator.getEstimatedBiasFx());
2200         assertNull(calibrator.getEstimatedBiasFy());
2201         assertNull(calibrator.getEstimatedBiasFz());
2202         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2203         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2204         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2205         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2206         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2207         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2208         assertNull(calibrator.getEstimatedMa());
2209         assertNull(calibrator.getEstimatedSx());
2210         assertNull(calibrator.getEstimatedSy());
2211         assertNull(calibrator.getEstimatedSz());
2212         assertNull(calibrator.getEstimatedMxy());
2213         assertNull(calibrator.getEstimatedMxz());
2214         assertNull(calibrator.getEstimatedMyx());
2215         assertNull(calibrator.getEstimatedMyz());
2216         assertNull(calibrator.getEstimatedMzx());
2217         assertNull(calibrator.getEstimatedMzy());
2218         assertNull(calibrator.getEstimatedCovariance());
2219         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2220         assertNull(calibrator.getGroundTruthGravityNorm());
2221         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2222         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2223     }
2224 
2225     @Test
2226     public void testConstructor22() throws WrongSizeException {
2227         final Matrix ba = generateBa();
2228         final double biasX = ba.getElementAtIndex(0);
2229         final double biasY = ba.getElementAtIndex(1);
2230         final double biasZ = ba.getElementAtIndex(2);
2231 
2232         final Acceleration bx = new Acceleration(biasX,
2233                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2234         final Acceleration by = new Acceleration(biasY,
2235                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2236         final Acceleration bz = new Acceleration(biasZ,
2237                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2238 
2239         final KnownPositionAccelerometerCalibrator calibrator =
2240                 new KnownPositionAccelerometerCalibrator(true,
2241                         bx, by, bz, this);
2242 
2243         // check default values
2244         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2245         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2246         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2247         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2248         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2249         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2250         final Acceleration bx2 = new Acceleration(0.0,
2251                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2252         calibrator.getInitialBiasXAsAcceleration(bx2);
2253         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2254         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2255         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2256         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2257         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2258         final Acceleration by2 = new Acceleration(0.0,
2259                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2260         calibrator.getInitialBiasYAsAcceleration(by2);
2261         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2262         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2263         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2264         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2265         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2266         final Acceleration bz2 = new Acceleration(0.0,
2267                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2268         calibrator.getInitialBiasZAsAcceleration(bz2);
2269         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2270         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2271         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2272         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2273         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2274         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2275         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2276         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2277         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2278         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2279         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2280         final double[] bias1 = calibrator.getInitialBias();
2281         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2282         final double[] bias2 = new double[3];
2283         calibrator.getInitialBias(bias2);
2284         assertArrayEquals(bias1, bias2, 0.0);
2285         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2286         assertEquals(b1, ba);
2287         final Matrix b2 = new Matrix(3, 1);
2288         calibrator.getInitialBiasAsMatrix(b2);
2289         assertEquals(b1, b2);
2290         final Matrix ma1 = calibrator.getInitialMa();
2291         assertEquals(ma1, new Matrix(3, 3));
2292         final Matrix ma2 = new Matrix(3, 3);
2293         calibrator.getInitialMa(ma2);
2294         assertEquals(ma1, ma2);
2295         assertNull(calibrator.getEcefPosition());
2296         assertNull(calibrator.getNedPosition());
2297         assertFalse(calibrator.getNedPosition(null));
2298         assertNull(calibrator.getMeasurements());
2299         assertTrue(calibrator.isCommonAxisUsed());
2300         assertSame(calibrator.getListener(), this);
2301         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2302         assertFalse(calibrator.isReady());
2303         assertFalse(calibrator.isRunning());
2304         assertNull(calibrator.getEstimatedBiases());
2305         assertFalse(calibrator.getEstimatedBiases(null));
2306         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2307         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2308         assertNull(calibrator.getEstimatedBiasFx());
2309         assertNull(calibrator.getEstimatedBiasFy());
2310         assertNull(calibrator.getEstimatedBiasFz());
2311         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2312         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2313         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2314         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2315         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2316         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2317         assertNull(calibrator.getEstimatedMa());
2318         assertNull(calibrator.getEstimatedSx());
2319         assertNull(calibrator.getEstimatedSy());
2320         assertNull(calibrator.getEstimatedSz());
2321         assertNull(calibrator.getEstimatedMxy());
2322         assertNull(calibrator.getEstimatedMxz());
2323         assertNull(calibrator.getEstimatedMyx());
2324         assertNull(calibrator.getEstimatedMyz());
2325         assertNull(calibrator.getEstimatedMzx());
2326         assertNull(calibrator.getEstimatedMzy());
2327         assertNull(calibrator.getEstimatedCovariance());
2328         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2329         assertNull(calibrator.getGroundTruthGravityNorm());
2330         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2331         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2332     }
2333 
2334     @Test
2335     public void testConstructor23() throws WrongSizeException {
2336         final Collection<StandardDeviationBodyKinematics> measurements =
2337                 Collections.emptyList();
2338 
2339         final Matrix ba = generateBa();
2340         final double biasX = ba.getElementAtIndex(0);
2341         final double biasY = ba.getElementAtIndex(1);
2342         final double biasZ = ba.getElementAtIndex(2);
2343 
2344         final Acceleration bx = new Acceleration(biasX,
2345                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2346         final Acceleration by = new Acceleration(biasY,
2347                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2348         final Acceleration bz = new Acceleration(biasZ,
2349                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2350 
2351         final KnownPositionAccelerometerCalibrator calibrator =
2352                 new KnownPositionAccelerometerCalibrator(measurements,
2353                         true, bx, by, bz);
2354 
2355         // check default values
2356         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2357         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2358         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2359         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2360         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2361         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2362         final Acceleration bx2 = new Acceleration(0.0,
2363                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2364         calibrator.getInitialBiasXAsAcceleration(bx2);
2365         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2366         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2367         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2368         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2369         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2370         final Acceleration by2 = new Acceleration(0.0,
2371                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2372         calibrator.getInitialBiasYAsAcceleration(by2);
2373         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2374         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2375         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2376         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2377         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2378         final Acceleration bz2 = new Acceleration(0.0,
2379                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2380         calibrator.getInitialBiasZAsAcceleration(bz2);
2381         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2382         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2383         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2384         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2385         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2386         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2387         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2388         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2389         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2390         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2391         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2392         final double[] bias1 = calibrator.getInitialBias();
2393         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2394         final double[] bias2 = new double[3];
2395         calibrator.getInitialBias(bias2);
2396         assertArrayEquals(bias1, bias2, 0.0);
2397         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2398         assertEquals(b1, ba);
2399         final Matrix b2 = new Matrix(3, 1);
2400         calibrator.getInitialBiasAsMatrix(b2);
2401         assertEquals(b1, b2);
2402         final Matrix ma1 = calibrator.getInitialMa();
2403         assertEquals(ma1, new Matrix(3, 3));
2404         final Matrix ma2 = new Matrix(3, 3);
2405         calibrator.getInitialMa(ma2);
2406         assertEquals(ma1, ma2);
2407         assertNull(calibrator.getEcefPosition());
2408         assertNull(calibrator.getNedPosition());
2409         assertFalse(calibrator.getNedPosition(null));
2410         assertSame(calibrator.getMeasurements(), measurements);
2411         assertTrue(calibrator.isCommonAxisUsed());
2412         assertNull(calibrator.getListener());
2413         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2414         assertFalse(calibrator.isReady());
2415         assertFalse(calibrator.isRunning());
2416         assertNull(calibrator.getEstimatedBiases());
2417         assertFalse(calibrator.getEstimatedBiases(null));
2418         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2419         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2420         assertNull(calibrator.getEstimatedBiasFx());
2421         assertNull(calibrator.getEstimatedBiasFy());
2422         assertNull(calibrator.getEstimatedBiasFz());
2423         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2424         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2425         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2426         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2427         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2428         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2429         assertNull(calibrator.getEstimatedMa());
2430         assertNull(calibrator.getEstimatedSx());
2431         assertNull(calibrator.getEstimatedSy());
2432         assertNull(calibrator.getEstimatedSz());
2433         assertNull(calibrator.getEstimatedMxy());
2434         assertNull(calibrator.getEstimatedMxz());
2435         assertNull(calibrator.getEstimatedMyx());
2436         assertNull(calibrator.getEstimatedMyz());
2437         assertNull(calibrator.getEstimatedMzx());
2438         assertNull(calibrator.getEstimatedMzy());
2439         assertNull(calibrator.getEstimatedCovariance());
2440         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2441         assertNull(calibrator.getGroundTruthGravityNorm());
2442         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2443         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2444     }
2445 
2446     @Test
2447     public void testConstructor24() throws WrongSizeException {
2448         final Collection<StandardDeviationBodyKinematics> measurements =
2449                 Collections.emptyList();
2450 
2451         final Matrix ba = generateBa();
2452         final double biasX = ba.getElementAtIndex(0);
2453         final double biasY = ba.getElementAtIndex(1);
2454         final double biasZ = ba.getElementAtIndex(2);
2455 
2456         final Acceleration bx = new Acceleration(biasX,
2457                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2458         final Acceleration by = new Acceleration(biasY,
2459                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2460         final Acceleration bz = new Acceleration(biasZ,
2461                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2462 
2463         final KnownPositionAccelerometerCalibrator calibrator =
2464                 new KnownPositionAccelerometerCalibrator(measurements,
2465                         true, bx, by, bz, this);
2466 
2467         // check default values
2468         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2469         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2470         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2471         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2472         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2473         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2474         final Acceleration bx2 = new Acceleration(0.0,
2475                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2476         calibrator.getInitialBiasXAsAcceleration(bx2);
2477         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2478         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2479         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2480         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2481         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2482         final Acceleration by2 = new Acceleration(0.0,
2483                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2484         calibrator.getInitialBiasYAsAcceleration(by2);
2485         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2486         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2487         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2488         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2489         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2490         final Acceleration bz2 = new Acceleration(0.0,
2491                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2492         calibrator.getInitialBiasZAsAcceleration(bz2);
2493         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2494         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2495         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2496         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2497         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2498         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2499         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2500         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2501         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2502         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2503         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2504         final double[] bias1 = calibrator.getInitialBias();
2505         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2506         final double[] bias2 = new double[3];
2507         calibrator.getInitialBias(bias2);
2508         assertArrayEquals(bias1, bias2, 0.0);
2509         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2510         assertEquals(b1, ba);
2511         final Matrix b2 = new Matrix(3, 1);
2512         calibrator.getInitialBiasAsMatrix(b2);
2513         assertEquals(b1, b2);
2514         final Matrix ma1 = calibrator.getInitialMa();
2515         assertEquals(ma1, new Matrix(3, 3));
2516         final Matrix ma2 = new Matrix(3, 3);
2517         calibrator.getInitialMa(ma2);
2518         assertEquals(ma1, ma2);
2519         assertNull(calibrator.getEcefPosition());
2520         assertNull(calibrator.getNedPosition());
2521         assertFalse(calibrator.getNedPosition(null));
2522         assertSame(calibrator.getMeasurements(), measurements);
2523         assertTrue(calibrator.isCommonAxisUsed());
2524         assertSame(calibrator.getListener(), this);
2525         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2526         assertFalse(calibrator.isReady());
2527         assertFalse(calibrator.isRunning());
2528         assertNull(calibrator.getEstimatedBiases());
2529         assertFalse(calibrator.getEstimatedBiases(null));
2530         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2531         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2532         assertNull(calibrator.getEstimatedBiasFx());
2533         assertNull(calibrator.getEstimatedBiasFy());
2534         assertNull(calibrator.getEstimatedBiasFz());
2535         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2536         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2537         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2538         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2539         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2540         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2541         assertNull(calibrator.getEstimatedMa());
2542         assertNull(calibrator.getEstimatedSx());
2543         assertNull(calibrator.getEstimatedSy());
2544         assertNull(calibrator.getEstimatedSz());
2545         assertNull(calibrator.getEstimatedMxy());
2546         assertNull(calibrator.getEstimatedMxz());
2547         assertNull(calibrator.getEstimatedMyx());
2548         assertNull(calibrator.getEstimatedMyz());
2549         assertNull(calibrator.getEstimatedMzx());
2550         assertNull(calibrator.getEstimatedMzy());
2551         assertNull(calibrator.getEstimatedCovariance());
2552         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2553         assertNull(calibrator.getGroundTruthGravityNorm());
2554         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2555         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2556     }
2557 
2558     @Test
2559     public void testConstructor25() throws WrongSizeException {
2560         final Matrix ba = generateBa();
2561         final double biasX = ba.getElementAtIndex(0);
2562         final double biasY = ba.getElementAtIndex(1);
2563         final double biasZ = ba.getElementAtIndex(2);
2564 
2565         final Matrix ma = generateMaCommonAxis();
2566         final double sx = ma.getElementAt(0, 0);
2567         final double sy = ma.getElementAt(1, 1);
2568         final double sz = ma.getElementAt(2, 2);
2569 
2570         final KnownPositionAccelerometerCalibrator calibrator =
2571                 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
2572                         sx, sy, sz);
2573 
2574         // check default values
2575         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2576         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2577         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2578         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2579         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2580         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2581         final Acceleration bx2 = new Acceleration(0.0,
2582                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2583         calibrator.getInitialBiasXAsAcceleration(bx2);
2584         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2585         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2586         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2587         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2588         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2589         final Acceleration by2 = new Acceleration(0.0,
2590                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2591         calibrator.getInitialBiasYAsAcceleration(by2);
2592         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2593         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2594         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2595         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2596         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2597         final Acceleration bz2 = new Acceleration(0.0,
2598                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2599         calibrator.getInitialBiasZAsAcceleration(bz2);
2600         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2601         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2602         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2603         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2604         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2605         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2606         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2607         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2608         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2609         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2610         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2611         final double[] bias1 = calibrator.getInitialBias();
2612         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2613         final double[] bias2 = new double[3];
2614         calibrator.getInitialBias(bias2);
2615         assertArrayEquals(bias1, bias2, 0.0);
2616         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2617         assertEquals(b1, ba);
2618         final Matrix b2 = new Matrix(3, 1);
2619         calibrator.getInitialBiasAsMatrix(b2);
2620         assertEquals(b1, b2);
2621         final Matrix ma1 = calibrator.getInitialMa();
2622         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2623         final Matrix ma2 = new Matrix(3, 3);
2624         calibrator.getInitialMa(ma2);
2625         assertEquals(ma1, ma2);
2626         assertNull(calibrator.getEcefPosition());
2627         assertNull(calibrator.getNedPosition());
2628         assertFalse(calibrator.getNedPosition(null));
2629         assertNull(calibrator.getMeasurements());
2630         assertFalse(calibrator.isCommonAxisUsed());
2631         assertNull(calibrator.getListener());
2632         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2633         assertFalse(calibrator.isReady());
2634         assertFalse(calibrator.isRunning());
2635         assertNull(calibrator.getEstimatedBiases());
2636         assertFalse(calibrator.getEstimatedBiases(null));
2637         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2638         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2639         assertNull(calibrator.getEstimatedBiasFx());
2640         assertNull(calibrator.getEstimatedBiasFy());
2641         assertNull(calibrator.getEstimatedBiasFz());
2642         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2643         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2644         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2645         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2646         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2647         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2648         assertNull(calibrator.getEstimatedMa());
2649         assertNull(calibrator.getEstimatedSx());
2650         assertNull(calibrator.getEstimatedSy());
2651         assertNull(calibrator.getEstimatedSz());
2652         assertNull(calibrator.getEstimatedMxy());
2653         assertNull(calibrator.getEstimatedMxz());
2654         assertNull(calibrator.getEstimatedMyx());
2655         assertNull(calibrator.getEstimatedMyz());
2656         assertNull(calibrator.getEstimatedMzx());
2657         assertNull(calibrator.getEstimatedMzy());
2658         assertNull(calibrator.getEstimatedCovariance());
2659         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2660         assertNull(calibrator.getGroundTruthGravityNorm());
2661         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2662         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2663     }
2664 
2665     @Test
2666     public void testConstructor26() throws WrongSizeException {
2667         final Collection<StandardDeviationBodyKinematics> measurements =
2668                 Collections.emptyList();
2669 
2670         final Matrix ba = generateBa();
2671         final double biasX = ba.getElementAtIndex(0);
2672         final double biasY = ba.getElementAtIndex(1);
2673         final double biasZ = ba.getElementAtIndex(2);
2674 
2675         final Matrix ma = generateMaCommonAxis();
2676         final double sx = ma.getElementAt(0, 0);
2677         final double sy = ma.getElementAt(1, 1);
2678         final double sz = ma.getElementAt(2, 2);
2679 
2680         final KnownPositionAccelerometerCalibrator calibrator =
2681                 new KnownPositionAccelerometerCalibrator(measurements,
2682                         biasX, biasY, biasZ, sx, sy, sz);
2683 
2684         // check default values
2685         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2686         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2687         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2688         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2689         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2690         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2691         final Acceleration bx2 = new Acceleration(0.0,
2692                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2693         calibrator.getInitialBiasXAsAcceleration(bx2);
2694         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2695         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2696         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2697         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2698         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2699         final Acceleration by2 = new Acceleration(0.0,
2700                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2701         calibrator.getInitialBiasYAsAcceleration(by2);
2702         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2703         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2704         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2705         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2706         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2707         final Acceleration bz2 = new Acceleration(0.0,
2708                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2709         calibrator.getInitialBiasZAsAcceleration(bz2);
2710         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2711         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2712         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2713         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2714         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2715         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2716         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2717         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2718         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2719         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2720         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2721         final double[] bias1 = calibrator.getInitialBias();
2722         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2723         final double[] bias2 = new double[3];
2724         calibrator.getInitialBias(bias2);
2725         assertArrayEquals(bias1, bias2, 0.0);
2726         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2727         assertEquals(b1, ba);
2728         final Matrix b2 = new Matrix(3, 1);
2729         calibrator.getInitialBiasAsMatrix(b2);
2730         assertEquals(b1, b2);
2731         final Matrix ma1 = calibrator.getInitialMa();
2732         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2733         final Matrix ma2 = new Matrix(3, 3);
2734         calibrator.getInitialMa(ma2);
2735         assertEquals(ma1, ma2);
2736         assertNull(calibrator.getEcefPosition());
2737         assertNull(calibrator.getNedPosition());
2738         assertFalse(calibrator.getNedPosition(null));
2739         assertSame(calibrator.getMeasurements(), measurements);
2740         assertFalse(calibrator.isCommonAxisUsed());
2741         assertNull(calibrator.getListener());
2742         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2743         assertFalse(calibrator.isReady());
2744         assertFalse(calibrator.isRunning());
2745         assertNull(calibrator.getEstimatedBiases());
2746         assertFalse(calibrator.getEstimatedBiases(null));
2747         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2748         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2749         assertNull(calibrator.getEstimatedBiasFx());
2750         assertNull(calibrator.getEstimatedBiasFy());
2751         assertNull(calibrator.getEstimatedBiasFz());
2752         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2753         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2754         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2755         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2756         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2757         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2758         assertNull(calibrator.getEstimatedMa());
2759         assertNull(calibrator.getEstimatedSx());
2760         assertNull(calibrator.getEstimatedSy());
2761         assertNull(calibrator.getEstimatedSz());
2762         assertNull(calibrator.getEstimatedMxy());
2763         assertNull(calibrator.getEstimatedMxz());
2764         assertNull(calibrator.getEstimatedMyx());
2765         assertNull(calibrator.getEstimatedMyz());
2766         assertNull(calibrator.getEstimatedMzx());
2767         assertNull(calibrator.getEstimatedMzy());
2768         assertNull(calibrator.getEstimatedCovariance());
2769         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2770         assertNull(calibrator.getGroundTruthGravityNorm());
2771         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2772         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2773     }
2774 
2775     @Test
2776     public void testConstructor27() throws WrongSizeException {
2777         final Collection<StandardDeviationBodyKinematics> measurements =
2778                 Collections.emptyList();
2779 
2780         final Matrix ba = generateBa();
2781         final double biasX = ba.getElementAtIndex(0);
2782         final double biasY = ba.getElementAtIndex(1);
2783         final double biasZ = ba.getElementAtIndex(2);
2784 
2785         final Matrix ma = generateMaCommonAxis();
2786         final double sx = ma.getElementAt(0, 0);
2787         final double sy = ma.getElementAt(1, 1);
2788         final double sz = ma.getElementAt(2, 2);
2789 
2790         final KnownPositionAccelerometerCalibrator calibrator =
2791                 new KnownPositionAccelerometerCalibrator(measurements,
2792                         biasX, biasY, biasZ, sx, sy, sz, this);
2793 
2794         // check default values
2795         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2796         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2797         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2798         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2799         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2800         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2801         final Acceleration bx2 = new Acceleration(0.0,
2802                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2803         calibrator.getInitialBiasXAsAcceleration(bx2);
2804         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2805         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2806         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2807         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2808         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2809         final Acceleration by2 = new Acceleration(0.0,
2810                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2811         calibrator.getInitialBiasYAsAcceleration(by2);
2812         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2813         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2814         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2815         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2816         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2817         final Acceleration bz2 = new Acceleration(0.0,
2818                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2819         calibrator.getInitialBiasZAsAcceleration(bz2);
2820         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2821         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2822         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2823         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2824         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2825         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2826         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2827         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2828         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2829         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2830         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2831         final double[] bias1 = calibrator.getInitialBias();
2832         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2833         final double[] bias2 = new double[3];
2834         calibrator.getInitialBias(bias2);
2835         assertArrayEquals(bias1, bias2, 0.0);
2836         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2837         assertEquals(b1, ba);
2838         final Matrix b2 = new Matrix(3, 1);
2839         calibrator.getInitialBiasAsMatrix(b2);
2840         assertEquals(b1, b2);
2841         final Matrix ma1 = calibrator.getInitialMa();
2842         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2843         final Matrix ma2 = new Matrix(3, 3);
2844         calibrator.getInitialMa(ma2);
2845         assertEquals(ma1, ma2);
2846         assertNull(calibrator.getEcefPosition());
2847         assertNull(calibrator.getNedPosition());
2848         assertFalse(calibrator.getNedPosition(null));
2849         assertSame(calibrator.getMeasurements(), measurements);
2850         assertFalse(calibrator.isCommonAxisUsed());
2851         assertSame(calibrator.getListener(), this);
2852         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2853         assertFalse(calibrator.isReady());
2854         assertFalse(calibrator.isRunning());
2855         assertNull(calibrator.getEstimatedBiases());
2856         assertFalse(calibrator.getEstimatedBiases(null));
2857         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2858         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2859         assertNull(calibrator.getEstimatedBiasFx());
2860         assertNull(calibrator.getEstimatedBiasFy());
2861         assertNull(calibrator.getEstimatedBiasFz());
2862         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2863         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2864         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2865         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2866         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2867         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2868         assertNull(calibrator.getEstimatedMa());
2869         assertNull(calibrator.getEstimatedSx());
2870         assertNull(calibrator.getEstimatedSy());
2871         assertNull(calibrator.getEstimatedSz());
2872         assertNull(calibrator.getEstimatedMxy());
2873         assertNull(calibrator.getEstimatedMxz());
2874         assertNull(calibrator.getEstimatedMyx());
2875         assertNull(calibrator.getEstimatedMyz());
2876         assertNull(calibrator.getEstimatedMzx());
2877         assertNull(calibrator.getEstimatedMzy());
2878         assertNull(calibrator.getEstimatedCovariance());
2879         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2880         assertNull(calibrator.getGroundTruthGravityNorm());
2881         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2882         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2883     }
2884 
2885     @Test
2886     public void testConstructor28() throws WrongSizeException {
2887         final Matrix ba = generateBa();
2888         final double biasX = ba.getElementAtIndex(0);
2889         final double biasY = ba.getElementAtIndex(1);
2890         final double biasZ = ba.getElementAtIndex(2);
2891 
2892         final Matrix ma = generateMaCommonAxis();
2893         final double sx = ma.getElementAt(0, 0);
2894         final double sy = ma.getElementAt(1, 1);
2895         final double sz = ma.getElementAt(2, 2);
2896 
2897         final KnownPositionAccelerometerCalibrator calibrator =
2898                 new KnownPositionAccelerometerCalibrator(true,
2899                         biasX, biasY, biasZ, sx, sy, sz);
2900 
2901         // check default values
2902         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2903         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2904         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2905         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2906         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2907         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2908         final Acceleration bx2 = new Acceleration(0.0,
2909                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2910         calibrator.getInitialBiasXAsAcceleration(bx2);
2911         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2912         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2913         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2914         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2915         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2916         final Acceleration by2 = new Acceleration(0.0,
2917                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2918         calibrator.getInitialBiasYAsAcceleration(by2);
2919         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2920         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2921         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2922         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2923         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2924         final Acceleration bz2 = new Acceleration(0.0,
2925                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2926         calibrator.getInitialBiasZAsAcceleration(bz2);
2927         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2928         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2929         assertEquals(calibrator.getInitialSx(), sx, 0.0);
2930         assertEquals(calibrator.getInitialSy(), sy, 0.0);
2931         assertEquals(calibrator.getInitialSz(), sz, 0.0);
2932         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2933         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2934         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2935         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2936         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2937         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2938         final double[] bias1 = calibrator.getInitialBias();
2939         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2940         final double[] bias2 = new double[3];
2941         calibrator.getInitialBias(bias2);
2942         assertArrayEquals(bias1, bias2, 0.0);
2943         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2944         assertEquals(b1, ba);
2945         final Matrix b2 = new Matrix(3, 1);
2946         calibrator.getInitialBiasAsMatrix(b2);
2947         assertEquals(b1, b2);
2948         final Matrix ma1 = calibrator.getInitialMa();
2949         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2950         final Matrix ma2 = new Matrix(3, 3);
2951         calibrator.getInitialMa(ma2);
2952         assertEquals(ma1, ma2);
2953         assertNull(calibrator.getEcefPosition());
2954         assertNull(calibrator.getNedPosition());
2955         assertFalse(calibrator.getNedPosition(null));
2956         assertNull(calibrator.getMeasurements());
2957         assertTrue(calibrator.isCommonAxisUsed());
2958         assertNull(calibrator.getListener());
2959         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2960         assertFalse(calibrator.isReady());
2961         assertFalse(calibrator.isRunning());
2962         assertNull(calibrator.getEstimatedBiases());
2963         assertFalse(calibrator.getEstimatedBiases(null));
2964         assertNull(calibrator.getEstimatedBiasesAsMatrix());
2965         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2966         assertNull(calibrator.getEstimatedBiasFx());
2967         assertNull(calibrator.getEstimatedBiasFy());
2968         assertNull(calibrator.getEstimatedBiasFz());
2969         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2970         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2971         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2972         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2973         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2974         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2975         assertNull(calibrator.getEstimatedMa());
2976         assertNull(calibrator.getEstimatedSx());
2977         assertNull(calibrator.getEstimatedSy());
2978         assertNull(calibrator.getEstimatedSz());
2979         assertNull(calibrator.getEstimatedMxy());
2980         assertNull(calibrator.getEstimatedMxz());
2981         assertNull(calibrator.getEstimatedMyx());
2982         assertNull(calibrator.getEstimatedMyz());
2983         assertNull(calibrator.getEstimatedMzx());
2984         assertNull(calibrator.getEstimatedMzy());
2985         assertNull(calibrator.getEstimatedCovariance());
2986         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2987         assertNull(calibrator.getGroundTruthGravityNorm());
2988         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2989         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2990     }
2991 
2992     @Test
2993     public void testConstructor29() throws WrongSizeException {
2994         final Matrix ba = generateBa();
2995         final double biasX = ba.getElementAtIndex(0);
2996         final double biasY = ba.getElementAtIndex(1);
2997         final double biasZ = ba.getElementAtIndex(2);
2998 
2999         final Matrix ma = generateMaCommonAxis();
3000         final double sx = ma.getElementAt(0, 0);
3001         final double sy = ma.getElementAt(1, 1);
3002         final double sz = ma.getElementAt(2, 2);
3003 
3004         final KnownPositionAccelerometerCalibrator calibrator =
3005                 new KnownPositionAccelerometerCalibrator(true,
3006                         biasX, biasY, biasZ, sx, sy, sz, this);
3007 
3008         // check default values
3009         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3010         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3011         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3012         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3013         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3014         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3015         final Acceleration bx2 = new Acceleration(0.0,
3016                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3017         calibrator.getInitialBiasXAsAcceleration(bx2);
3018         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3019         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3020         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3021         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3022         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3023         final Acceleration by2 = new Acceleration(0.0,
3024                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3025         calibrator.getInitialBiasYAsAcceleration(by2);
3026         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3027         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3028         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3029         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3030         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3031         final Acceleration bz2 = new Acceleration(0.0,
3032                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3033         calibrator.getInitialBiasZAsAcceleration(bz2);
3034         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3035         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3036         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3037         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3038         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3039         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3040         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3041         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3042         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3043         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3044         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3045         final double[] bias1 = calibrator.getInitialBias();
3046         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3047         final double[] bias2 = new double[3];
3048         calibrator.getInitialBias(bias2);
3049         assertArrayEquals(bias1, bias2, 0.0);
3050         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3051         assertEquals(b1, ba);
3052         final Matrix b2 = new Matrix(3, 1);
3053         calibrator.getInitialBiasAsMatrix(b2);
3054         assertEquals(b1, b2);
3055         final Matrix ma1 = calibrator.getInitialMa();
3056         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3057         final Matrix ma2 = new Matrix(3, 3);
3058         calibrator.getInitialMa(ma2);
3059         assertEquals(ma1, ma2);
3060         assertNull(calibrator.getEcefPosition());
3061         assertNull(calibrator.getNedPosition());
3062         assertFalse(calibrator.getNedPosition(null));
3063         assertNull(calibrator.getMeasurements());
3064         assertTrue(calibrator.isCommonAxisUsed());
3065         assertSame(calibrator.getListener(), this);
3066         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3067         assertFalse(calibrator.isReady());
3068         assertFalse(calibrator.isRunning());
3069         assertNull(calibrator.getEstimatedBiases());
3070         assertFalse(calibrator.getEstimatedBiases(null));
3071         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3072         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3073         assertNull(calibrator.getEstimatedBiasFx());
3074         assertNull(calibrator.getEstimatedBiasFy());
3075         assertNull(calibrator.getEstimatedBiasFz());
3076         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3077         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3078         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3079         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3080         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3081         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3082         assertNull(calibrator.getEstimatedMa());
3083         assertNull(calibrator.getEstimatedSx());
3084         assertNull(calibrator.getEstimatedSy());
3085         assertNull(calibrator.getEstimatedSz());
3086         assertNull(calibrator.getEstimatedMxy());
3087         assertNull(calibrator.getEstimatedMxz());
3088         assertNull(calibrator.getEstimatedMyx());
3089         assertNull(calibrator.getEstimatedMyz());
3090         assertNull(calibrator.getEstimatedMzx());
3091         assertNull(calibrator.getEstimatedMzy());
3092         assertNull(calibrator.getEstimatedCovariance());
3093         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3094         assertNull(calibrator.getGroundTruthGravityNorm());
3095         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3096         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3097     }
3098 
3099     @Test
3100     public void testConstructor30() throws WrongSizeException {
3101         final Collection<StandardDeviationBodyKinematics> measurements =
3102                 Collections.emptyList();
3103         final Matrix ba = generateBa();
3104         final double biasX = ba.getElementAtIndex(0);
3105         final double biasY = ba.getElementAtIndex(1);
3106         final double biasZ = ba.getElementAtIndex(2);
3107 
3108         final Matrix ma = generateMaCommonAxis();
3109         final double sx = ma.getElementAt(0, 0);
3110         final double sy = ma.getElementAt(1, 1);
3111         final double sz = ma.getElementAt(2, 2);
3112 
3113         final KnownPositionAccelerometerCalibrator calibrator =
3114                 new KnownPositionAccelerometerCalibrator(measurements,
3115                         true, biasX, biasY, biasZ, sx, sy, sz);
3116 
3117         // check default values
3118         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3119         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3120         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3121         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3122         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3123         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3124         final Acceleration bx2 = new Acceleration(0.0,
3125                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3126         calibrator.getInitialBiasXAsAcceleration(bx2);
3127         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3128         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3129         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3130         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3131         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3132         final Acceleration by2 = new Acceleration(0.0,
3133                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3134         calibrator.getInitialBiasYAsAcceleration(by2);
3135         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3136         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3137         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3138         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3139         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3140         final Acceleration bz2 = new Acceleration(0.0,
3141                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3142         calibrator.getInitialBiasZAsAcceleration(bz2);
3143         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3144         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3145         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3146         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3147         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3148         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3149         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3150         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3151         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3152         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3153         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3154         final double[] bias1 = calibrator.getInitialBias();
3155         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3156         final double[] bias2 = new double[3];
3157         calibrator.getInitialBias(bias2);
3158         assertArrayEquals(bias1, bias2, 0.0);
3159         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3160         assertEquals(b1, ba);
3161         final Matrix b2 = new Matrix(3, 1);
3162         calibrator.getInitialBiasAsMatrix(b2);
3163         assertEquals(b1, b2);
3164         final Matrix ma1 = calibrator.getInitialMa();
3165         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3166         final Matrix ma2 = new Matrix(3, 3);
3167         calibrator.getInitialMa(ma2);
3168         assertEquals(ma1, ma2);
3169         assertNull(calibrator.getEcefPosition());
3170         assertNull(calibrator.getNedPosition());
3171         assertFalse(calibrator.getNedPosition(null));
3172         assertSame(calibrator.getMeasurements(), measurements);
3173         assertTrue(calibrator.isCommonAxisUsed());
3174         assertNull(calibrator.getListener());
3175         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3176         assertFalse(calibrator.isReady());
3177         assertFalse(calibrator.isRunning());
3178         assertNull(calibrator.getEstimatedBiases());
3179         assertFalse(calibrator.getEstimatedBiases(null));
3180         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3181         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3182         assertNull(calibrator.getEstimatedBiasFx());
3183         assertNull(calibrator.getEstimatedBiasFy());
3184         assertNull(calibrator.getEstimatedBiasFz());
3185         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3186         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3187         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3188         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3189         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3190         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3191         assertNull(calibrator.getEstimatedMa());
3192         assertNull(calibrator.getEstimatedSx());
3193         assertNull(calibrator.getEstimatedSy());
3194         assertNull(calibrator.getEstimatedSz());
3195         assertNull(calibrator.getEstimatedMxy());
3196         assertNull(calibrator.getEstimatedMxz());
3197         assertNull(calibrator.getEstimatedMyx());
3198         assertNull(calibrator.getEstimatedMyz());
3199         assertNull(calibrator.getEstimatedMzx());
3200         assertNull(calibrator.getEstimatedMzy());
3201         assertNull(calibrator.getEstimatedCovariance());
3202         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3203         assertNull(calibrator.getGroundTruthGravityNorm());
3204         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3205         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3206     }
3207 
3208     @Test
3209     public void testConstructor31() throws WrongSizeException {
3210         final Collection<StandardDeviationBodyKinematics> measurements =
3211                 Collections.emptyList();
3212         final Matrix ba = generateBa();
3213         final double biasX = ba.getElementAtIndex(0);
3214         final double biasY = ba.getElementAtIndex(1);
3215         final double biasZ = ba.getElementAtIndex(2);
3216 
3217         final Matrix ma = generateMaCommonAxis();
3218         final double sx = ma.getElementAt(0, 0);
3219         final double sy = ma.getElementAt(1, 1);
3220         final double sz = ma.getElementAt(2, 2);
3221 
3222         final KnownPositionAccelerometerCalibrator calibrator =
3223                 new KnownPositionAccelerometerCalibrator(measurements,
3224                         true, biasX, biasY, biasZ, sx, sy, sz,
3225                         this);
3226 
3227         // check default values
3228         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3229         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3230         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3231         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3232         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3233         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3234         final Acceleration bx2 = new Acceleration(0.0,
3235                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3236         calibrator.getInitialBiasXAsAcceleration(bx2);
3237         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3238         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3239         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3240         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3241         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3242         final Acceleration by2 = new Acceleration(0.0,
3243                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3244         calibrator.getInitialBiasYAsAcceleration(by2);
3245         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3246         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3247         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3248         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3249         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3250         final Acceleration bz2 = new Acceleration(0.0,
3251                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3252         calibrator.getInitialBiasZAsAcceleration(bz2);
3253         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3254         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3255         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3256         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3257         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3258         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3259         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3260         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3261         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3262         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3263         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3264         final double[] bias1 = calibrator.getInitialBias();
3265         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3266         final double[] bias2 = new double[3];
3267         calibrator.getInitialBias(bias2);
3268         assertArrayEquals(bias1, bias2, 0.0);
3269         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3270         assertEquals(b1, ba);
3271         final Matrix b2 = new Matrix(3, 1);
3272         calibrator.getInitialBiasAsMatrix(b2);
3273         assertEquals(b1, b2);
3274         final Matrix ma1 = calibrator.getInitialMa();
3275         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3276         final Matrix ma2 = new Matrix(3, 3);
3277         calibrator.getInitialMa(ma2);
3278         assertEquals(ma1, ma2);
3279         assertNull(calibrator.getEcefPosition());
3280         assertNull(calibrator.getNedPosition());
3281         assertFalse(calibrator.getNedPosition(null));
3282         assertSame(calibrator.getMeasurements(), measurements);
3283         assertTrue(calibrator.isCommonAxisUsed());
3284         assertSame(calibrator.getListener(), this);
3285         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3286         assertFalse(calibrator.isReady());
3287         assertFalse(calibrator.isRunning());
3288         assertNull(calibrator.getEstimatedBiases());
3289         assertFalse(calibrator.getEstimatedBiases(null));
3290         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3291         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3292         assertNull(calibrator.getEstimatedBiasFx());
3293         assertNull(calibrator.getEstimatedBiasFy());
3294         assertNull(calibrator.getEstimatedBiasFz());
3295         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3296         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3297         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3298         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3299         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3300         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3301         assertNull(calibrator.getEstimatedMa());
3302         assertNull(calibrator.getEstimatedSx());
3303         assertNull(calibrator.getEstimatedSy());
3304         assertNull(calibrator.getEstimatedSz());
3305         assertNull(calibrator.getEstimatedMxy());
3306         assertNull(calibrator.getEstimatedMxz());
3307         assertNull(calibrator.getEstimatedMyx());
3308         assertNull(calibrator.getEstimatedMyz());
3309         assertNull(calibrator.getEstimatedMzx());
3310         assertNull(calibrator.getEstimatedMzy());
3311         assertNull(calibrator.getEstimatedCovariance());
3312         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3313         assertNull(calibrator.getGroundTruthGravityNorm());
3314         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3315         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3316     }
3317 
3318     @Test
3319     public void testConstructor32() throws WrongSizeException {
3320         final Matrix ba = generateBa();
3321         final double biasX = ba.getElementAtIndex(0);
3322         final double biasY = ba.getElementAtIndex(1);
3323         final double biasZ = ba.getElementAtIndex(2);
3324 
3325         final Matrix ma = generateMaCommonAxis();
3326         final double sx = ma.getElementAt(0, 0);
3327         final double sy = ma.getElementAt(1, 1);
3328         final double sz = ma.getElementAt(2, 2);
3329 
3330         final Acceleration bx = new Acceleration(biasX,
3331                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3332         final Acceleration by = new Acceleration(biasY,
3333                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3334         final Acceleration bz = new Acceleration(biasZ,
3335                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3336 
3337         final KnownPositionAccelerometerCalibrator calibrator =
3338                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
3339                         sx, sy, sz);
3340 
3341         // check default values
3342         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3343         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3344         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3345         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3346         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3347         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3348         final Acceleration bx2 = new Acceleration(0.0,
3349                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3350         calibrator.getInitialBiasXAsAcceleration(bx2);
3351         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3352         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3353         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3354         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3355         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3356         final Acceleration by2 = new Acceleration(0.0,
3357                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3358         calibrator.getInitialBiasYAsAcceleration(by2);
3359         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3360         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3361         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3362         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3363         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3364         final Acceleration bz2 = new Acceleration(0.0,
3365                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3366         calibrator.getInitialBiasZAsAcceleration(bz2);
3367         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3368         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3369         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3370         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3371         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3372         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3373         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3374         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3375         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3376         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3377         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3378         final double[] bias1 = calibrator.getInitialBias();
3379         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3380         final double[] bias2 = new double[3];
3381         calibrator.getInitialBias(bias2);
3382         assertArrayEquals(bias1, bias2, 0.0);
3383         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3384         assertEquals(b1, ba);
3385         final Matrix b2 = new Matrix(3, 1);
3386         calibrator.getInitialBiasAsMatrix(b2);
3387         assertEquals(b1, b2);
3388         final Matrix ma1 = calibrator.getInitialMa();
3389         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3390         final Matrix ma2 = new Matrix(3, 3);
3391         calibrator.getInitialMa(ma2);
3392         assertEquals(ma1, ma2);
3393         assertNull(calibrator.getEcefPosition());
3394         assertNull(calibrator.getNedPosition());
3395         assertFalse(calibrator.getNedPosition(null));
3396         assertNull(calibrator.getMeasurements());
3397         assertFalse(calibrator.isCommonAxisUsed());
3398         assertNull(calibrator.getListener());
3399         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3400         assertFalse(calibrator.isReady());
3401         assertFalse(calibrator.isRunning());
3402         assertNull(calibrator.getEstimatedBiases());
3403         assertFalse(calibrator.getEstimatedBiases(null));
3404         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3405         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3406         assertNull(calibrator.getEstimatedBiasFx());
3407         assertNull(calibrator.getEstimatedBiasFy());
3408         assertNull(calibrator.getEstimatedBiasFz());
3409         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3410         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3411         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3412         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3413         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3414         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3415         assertNull(calibrator.getEstimatedMa());
3416         assertNull(calibrator.getEstimatedSx());
3417         assertNull(calibrator.getEstimatedSy());
3418         assertNull(calibrator.getEstimatedSz());
3419         assertNull(calibrator.getEstimatedMxy());
3420         assertNull(calibrator.getEstimatedMxz());
3421         assertNull(calibrator.getEstimatedMyx());
3422         assertNull(calibrator.getEstimatedMyz());
3423         assertNull(calibrator.getEstimatedMzx());
3424         assertNull(calibrator.getEstimatedMzy());
3425         assertNull(calibrator.getEstimatedCovariance());
3426         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3427         assertNull(calibrator.getGroundTruthGravityNorm());
3428         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3429         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3430     }
3431 
3432     @Test
3433     public void testConstructor33() throws WrongSizeException {
3434         final Matrix ba = generateBa();
3435         final double biasX = ba.getElementAtIndex(0);
3436         final double biasY = ba.getElementAtIndex(1);
3437         final double biasZ = ba.getElementAtIndex(2);
3438 
3439         final Matrix ma = generateMaCommonAxis();
3440         final double sx = ma.getElementAt(0, 0);
3441         final double sy = ma.getElementAt(1, 1);
3442         final double sz = ma.getElementAt(2, 2);
3443 
3444         final Acceleration bx = new Acceleration(biasX,
3445                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3446         final Acceleration by = new Acceleration(biasY,
3447                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3448         final Acceleration bz = new Acceleration(biasZ,
3449                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3450 
3451         final KnownPositionAccelerometerCalibrator calibrator =
3452                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
3453                         sx, sy, sz, this);
3454 
3455         // check default values
3456         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3457         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3458         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3459         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3460         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3461         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3462         final Acceleration bx2 = new Acceleration(0.0,
3463                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3464         calibrator.getInitialBiasXAsAcceleration(bx2);
3465         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3466         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3467         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3468         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3469         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3470         final Acceleration by2 = new Acceleration(0.0,
3471                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3472         calibrator.getInitialBiasYAsAcceleration(by2);
3473         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3474         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3475         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3476         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3477         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3478         final Acceleration bz2 = new Acceleration(0.0,
3479                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3480         calibrator.getInitialBiasZAsAcceleration(bz2);
3481         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3482         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3483         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3484         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3485         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3486         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3487         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3488         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3489         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3490         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3491         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3492         final double[] bias1 = calibrator.getInitialBias();
3493         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3494         final double[] bias2 = new double[3];
3495         calibrator.getInitialBias(bias2);
3496         assertArrayEquals(bias1, bias2, 0.0);
3497         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3498         assertEquals(b1, ba);
3499         final Matrix b2 = new Matrix(3, 1);
3500         calibrator.getInitialBiasAsMatrix(b2);
3501         assertEquals(b1, b2);
3502         final Matrix ma1 = calibrator.getInitialMa();
3503         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3504         final Matrix ma2 = new Matrix(3, 3);
3505         calibrator.getInitialMa(ma2);
3506         assertEquals(ma1, ma2);
3507         assertNull(calibrator.getEcefPosition());
3508         assertNull(calibrator.getNedPosition());
3509         assertFalse(calibrator.getNedPosition(null));
3510         assertNull(calibrator.getMeasurements());
3511         assertFalse(calibrator.isCommonAxisUsed());
3512         assertSame(calibrator.getListener(), this);
3513         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3514         assertFalse(calibrator.isReady());
3515         assertFalse(calibrator.isRunning());
3516         assertNull(calibrator.getEstimatedBiases());
3517         assertFalse(calibrator.getEstimatedBiases(null));
3518         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3519         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3520         assertNull(calibrator.getEstimatedBiasFx());
3521         assertNull(calibrator.getEstimatedBiasFy());
3522         assertNull(calibrator.getEstimatedBiasFz());
3523         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3524         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3525         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3526         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3527         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3528         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3529         assertNull(calibrator.getEstimatedMa());
3530         assertNull(calibrator.getEstimatedSx());
3531         assertNull(calibrator.getEstimatedSy());
3532         assertNull(calibrator.getEstimatedSz());
3533         assertNull(calibrator.getEstimatedMxy());
3534         assertNull(calibrator.getEstimatedMxz());
3535         assertNull(calibrator.getEstimatedMyx());
3536         assertNull(calibrator.getEstimatedMyz());
3537         assertNull(calibrator.getEstimatedMzx());
3538         assertNull(calibrator.getEstimatedMzy());
3539         assertNull(calibrator.getEstimatedCovariance());
3540         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3541         assertNull(calibrator.getGroundTruthGravityNorm());
3542         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3543         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3544     }
3545 
3546     @Test
3547     public void testConstructor34() throws WrongSizeException {
3548         final Collection<StandardDeviationBodyKinematics> measurements =
3549                 Collections.emptyList();
3550 
3551         final Matrix ba = generateBa();
3552         final double biasX = ba.getElementAtIndex(0);
3553         final double biasY = ba.getElementAtIndex(1);
3554         final double biasZ = ba.getElementAtIndex(2);
3555 
3556         final Matrix ma = generateMaCommonAxis();
3557         final double sx = ma.getElementAt(0, 0);
3558         final double sy = ma.getElementAt(1, 1);
3559         final double sz = ma.getElementAt(2, 2);
3560 
3561         final Acceleration bx = new Acceleration(biasX,
3562                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3563         final Acceleration by = new Acceleration(biasY,
3564                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3565         final Acceleration bz = new Acceleration(biasZ,
3566                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3567 
3568         final KnownPositionAccelerometerCalibrator calibrator =
3569                 new KnownPositionAccelerometerCalibrator(measurements,
3570                         bx, by, bz, sx, sy, sz);
3571 
3572         // check default values
3573         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3574         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3575         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3576         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3577         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3578         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3579         final Acceleration bx2 = new Acceleration(0.0,
3580                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3581         calibrator.getInitialBiasXAsAcceleration(bx2);
3582         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3583         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3584         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3585         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3586         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3587         final Acceleration by2 = new Acceleration(0.0,
3588                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3589         calibrator.getInitialBiasYAsAcceleration(by2);
3590         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3591         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3592         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3593         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3594         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3595         final Acceleration bz2 = new Acceleration(0.0,
3596                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3597         calibrator.getInitialBiasZAsAcceleration(bz2);
3598         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3599         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3600         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3601         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3602         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3603         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3604         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3605         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3606         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3607         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3608         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3609         final double[] bias1 = calibrator.getInitialBias();
3610         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3611         final double[] bias2 = new double[3];
3612         calibrator.getInitialBias(bias2);
3613         assertArrayEquals(bias1, bias2, 0.0);
3614         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3615         assertEquals(b1, ba);
3616         final Matrix b2 = new Matrix(3, 1);
3617         calibrator.getInitialBiasAsMatrix(b2);
3618         assertEquals(b1, b2);
3619         final Matrix ma1 = calibrator.getInitialMa();
3620         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3621         final Matrix ma2 = new Matrix(3, 3);
3622         calibrator.getInitialMa(ma2);
3623         assertEquals(ma1, ma2);
3624         assertNull(calibrator.getEcefPosition());
3625         assertNull(calibrator.getNedPosition());
3626         assertFalse(calibrator.getNedPosition(null));
3627         assertSame(calibrator.getMeasurements(), measurements);
3628         assertFalse(calibrator.isCommonAxisUsed());
3629         assertNull(calibrator.getListener());
3630         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3631         assertFalse(calibrator.isReady());
3632         assertFalse(calibrator.isRunning());
3633         assertNull(calibrator.getEstimatedBiases());
3634         assertFalse(calibrator.getEstimatedBiases(null));
3635         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3636         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3637         assertNull(calibrator.getEstimatedBiasFx());
3638         assertNull(calibrator.getEstimatedBiasFy());
3639         assertNull(calibrator.getEstimatedBiasFz());
3640         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3641         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3642         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3643         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3644         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3645         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3646         assertNull(calibrator.getEstimatedMa());
3647         assertNull(calibrator.getEstimatedSx());
3648         assertNull(calibrator.getEstimatedSy());
3649         assertNull(calibrator.getEstimatedSz());
3650         assertNull(calibrator.getEstimatedMxy());
3651         assertNull(calibrator.getEstimatedMxz());
3652         assertNull(calibrator.getEstimatedMyx());
3653         assertNull(calibrator.getEstimatedMyz());
3654         assertNull(calibrator.getEstimatedMzx());
3655         assertNull(calibrator.getEstimatedMzy());
3656         assertNull(calibrator.getEstimatedCovariance());
3657         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3658         assertNull(calibrator.getGroundTruthGravityNorm());
3659         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3660         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3661     }
3662 
3663     @Test
3664     public void testConstructor35() throws WrongSizeException {
3665         final Collection<StandardDeviationBodyKinematics> measurements =
3666                 Collections.emptyList();
3667 
3668         final Matrix ba = generateBa();
3669         final double biasX = ba.getElementAtIndex(0);
3670         final double biasY = ba.getElementAtIndex(1);
3671         final double biasZ = ba.getElementAtIndex(2);
3672 
3673         final Matrix ma = generateMaCommonAxis();
3674         final double sx = ma.getElementAt(0, 0);
3675         final double sy = ma.getElementAt(1, 1);
3676         final double sz = ma.getElementAt(2, 2);
3677 
3678         final Acceleration bx = new Acceleration(biasX,
3679                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3680         final Acceleration by = new Acceleration(biasY,
3681                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3682         final Acceleration bz = new Acceleration(biasZ,
3683                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3684 
3685         final KnownPositionAccelerometerCalibrator calibrator =
3686                 new KnownPositionAccelerometerCalibrator(measurements,
3687                         bx, by, bz, sx, sy, sz, this);
3688 
3689         // check default values
3690         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3691         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3692         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3693         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3694         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3695         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3696         final Acceleration bx2 = new Acceleration(0.0,
3697                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3698         calibrator.getInitialBiasXAsAcceleration(bx2);
3699         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3700         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3701         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3702         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3703         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3704         final Acceleration by2 = new Acceleration(0.0,
3705                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3706         calibrator.getInitialBiasYAsAcceleration(by2);
3707         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3708         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3709         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3710         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3711         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3712         final Acceleration bz2 = new Acceleration(0.0,
3713                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3714         calibrator.getInitialBiasZAsAcceleration(bz2);
3715         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3716         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3717         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3718         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3719         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3720         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3721         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3722         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3723         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3724         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3725         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3726         final double[] bias1 = calibrator.getInitialBias();
3727         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3728         final double[] bias2 = new double[3];
3729         calibrator.getInitialBias(bias2);
3730         assertArrayEquals(bias1, bias2, 0.0);
3731         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3732         assertEquals(b1, ba);
3733         final Matrix b2 = new Matrix(3, 1);
3734         calibrator.getInitialBiasAsMatrix(b2);
3735         assertEquals(b1, b2);
3736         final Matrix ma1 = calibrator.getInitialMa();
3737         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3738         final Matrix ma2 = new Matrix(3, 3);
3739         calibrator.getInitialMa(ma2);
3740         assertEquals(ma1, ma2);
3741         assertNull(calibrator.getEcefPosition());
3742         assertNull(calibrator.getNedPosition());
3743         assertFalse(calibrator.getNedPosition(null));
3744         assertSame(calibrator.getMeasurements(), measurements);
3745         assertFalse(calibrator.isCommonAxisUsed());
3746         assertSame(calibrator.getListener(), this);
3747         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3748         assertFalse(calibrator.isReady());
3749         assertFalse(calibrator.isRunning());
3750         assertNull(calibrator.getEstimatedBiases());
3751         assertFalse(calibrator.getEstimatedBiases(null));
3752         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3753         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3754         assertNull(calibrator.getEstimatedBiasFx());
3755         assertNull(calibrator.getEstimatedBiasFy());
3756         assertNull(calibrator.getEstimatedBiasFz());
3757         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3758         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3759         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3760         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3761         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3762         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3763         assertNull(calibrator.getEstimatedMa());
3764         assertNull(calibrator.getEstimatedSx());
3765         assertNull(calibrator.getEstimatedSy());
3766         assertNull(calibrator.getEstimatedSz());
3767         assertNull(calibrator.getEstimatedMxy());
3768         assertNull(calibrator.getEstimatedMxz());
3769         assertNull(calibrator.getEstimatedMyx());
3770         assertNull(calibrator.getEstimatedMyz());
3771         assertNull(calibrator.getEstimatedMzx());
3772         assertNull(calibrator.getEstimatedMzy());
3773         assertNull(calibrator.getEstimatedCovariance());
3774         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3775         assertNull(calibrator.getGroundTruthGravityNorm());
3776         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3777         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3778     }
3779 
3780     @Test
3781     public void testConstructor36() throws WrongSizeException {
3782         final Matrix ba = generateBa();
3783         final double biasX = ba.getElementAtIndex(0);
3784         final double biasY = ba.getElementAtIndex(1);
3785         final double biasZ = ba.getElementAtIndex(2);
3786 
3787         final Matrix ma = generateMaCommonAxis();
3788         final double sx = ma.getElementAt(0, 0);
3789         final double sy = ma.getElementAt(1, 1);
3790         final double sz = ma.getElementAt(2, 2);
3791 
3792         final Acceleration bx = new Acceleration(biasX,
3793                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3794         final Acceleration by = new Acceleration(biasY,
3795                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3796         final Acceleration bz = new Acceleration(biasZ,
3797                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3798 
3799         final KnownPositionAccelerometerCalibrator calibrator =
3800                 new KnownPositionAccelerometerCalibrator(true,
3801                         bx, by, bz, sx, sy, sz);
3802 
3803         // check default values
3804         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3805         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3806         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3807         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3808         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3809         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3810         final Acceleration bx2 = new Acceleration(0.0,
3811                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3812         calibrator.getInitialBiasXAsAcceleration(bx2);
3813         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3814         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3815         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3816         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3817         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3818         final Acceleration by2 = new Acceleration(0.0,
3819                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3820         calibrator.getInitialBiasYAsAcceleration(by2);
3821         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3822         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3823         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3824         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3825         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3826         final Acceleration bz2 = new Acceleration(0.0,
3827                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3828         calibrator.getInitialBiasZAsAcceleration(bz2);
3829         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3830         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3831         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3832         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3833         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3834         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3835         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3836         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3837         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3838         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3839         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3840         final double[] bias1 = calibrator.getInitialBias();
3841         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3842         final double[] bias2 = new double[3];
3843         calibrator.getInitialBias(bias2);
3844         assertArrayEquals(bias1, bias2, 0.0);
3845         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3846         assertEquals(b1, ba);
3847         final Matrix b2 = new Matrix(3, 1);
3848         calibrator.getInitialBiasAsMatrix(b2);
3849         assertEquals(b1, b2);
3850         final Matrix ma1 = calibrator.getInitialMa();
3851         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3852         final Matrix ma2 = new Matrix(3, 3);
3853         calibrator.getInitialMa(ma2);
3854         assertEquals(ma1, ma2);
3855         assertNull(calibrator.getEcefPosition());
3856         assertNull(calibrator.getNedPosition());
3857         assertFalse(calibrator.getNedPosition(null));
3858         assertNull(calibrator.getMeasurements());
3859         assertTrue(calibrator.isCommonAxisUsed());
3860         assertNull(calibrator.getListener());
3861         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3862         assertFalse(calibrator.isReady());
3863         assertFalse(calibrator.isRunning());
3864         assertNull(calibrator.getEstimatedBiases());
3865         assertFalse(calibrator.getEstimatedBiases(null));
3866         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3867         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3868         assertNull(calibrator.getEstimatedBiasFx());
3869         assertNull(calibrator.getEstimatedBiasFy());
3870         assertNull(calibrator.getEstimatedBiasFz());
3871         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3872         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3873         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3874         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3875         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3876         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3877         assertNull(calibrator.getEstimatedMa());
3878         assertNull(calibrator.getEstimatedSx());
3879         assertNull(calibrator.getEstimatedSy());
3880         assertNull(calibrator.getEstimatedSz());
3881         assertNull(calibrator.getEstimatedMxy());
3882         assertNull(calibrator.getEstimatedMxz());
3883         assertNull(calibrator.getEstimatedMyx());
3884         assertNull(calibrator.getEstimatedMyz());
3885         assertNull(calibrator.getEstimatedMzx());
3886         assertNull(calibrator.getEstimatedMzy());
3887         assertNull(calibrator.getEstimatedCovariance());
3888         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3889         assertNull(calibrator.getGroundTruthGravityNorm());
3890         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3891         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3892     }
3893 
3894     @Test
3895     public void testConstructor37() throws WrongSizeException {
3896         final Matrix ba = generateBa();
3897         final double biasX = ba.getElementAtIndex(0);
3898         final double biasY = ba.getElementAtIndex(1);
3899         final double biasZ = ba.getElementAtIndex(2);
3900 
3901         final Matrix ma = generateMaCommonAxis();
3902         final double sx = ma.getElementAt(0, 0);
3903         final double sy = ma.getElementAt(1, 1);
3904         final double sz = ma.getElementAt(2, 2);
3905 
3906         final Acceleration bx = new Acceleration(biasX,
3907                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3908         final Acceleration by = new Acceleration(biasY,
3909                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3910         final Acceleration bz = new Acceleration(biasZ,
3911                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3912 
3913         final KnownPositionAccelerometerCalibrator calibrator =
3914                 new KnownPositionAccelerometerCalibrator(true,
3915                         bx, by, bz, sx, sy, sz, this);
3916 
3917         // check default values
3918         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3919         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3920         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3921         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3922         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3923         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3924         final Acceleration bx2 = new Acceleration(0.0,
3925                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3926         calibrator.getInitialBiasXAsAcceleration(bx2);
3927         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3928         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3929         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3930         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3931         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3932         final Acceleration by2 = new Acceleration(0.0,
3933                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3934         calibrator.getInitialBiasYAsAcceleration(by2);
3935         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3936         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3937         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3938         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3939         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3940         final Acceleration bz2 = new Acceleration(0.0,
3941                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3942         calibrator.getInitialBiasZAsAcceleration(bz2);
3943         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3944         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3945         assertEquals(calibrator.getInitialSx(), sx, 0.0);
3946         assertEquals(calibrator.getInitialSy(), sy, 0.0);
3947         assertEquals(calibrator.getInitialSz(), sz, 0.0);
3948         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3949         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3950         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3951         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3952         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3953         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3954         final double[] bias1 = calibrator.getInitialBias();
3955         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3956         final double[] bias2 = new double[3];
3957         calibrator.getInitialBias(bias2);
3958         assertArrayEquals(bias1, bias2, 0.0);
3959         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3960         assertEquals(b1, ba);
3961         final Matrix b2 = new Matrix(3, 1);
3962         calibrator.getInitialBiasAsMatrix(b2);
3963         assertEquals(b1, b2);
3964         final Matrix ma1 = calibrator.getInitialMa();
3965         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3966         final Matrix ma2 = new Matrix(3, 3);
3967         calibrator.getInitialMa(ma2);
3968         assertEquals(ma1, ma2);
3969         assertNull(calibrator.getEcefPosition());
3970         assertNull(calibrator.getNedPosition());
3971         assertFalse(calibrator.getNedPosition(null));
3972         assertNull(calibrator.getMeasurements());
3973         assertTrue(calibrator.isCommonAxisUsed());
3974         assertSame(calibrator.getListener(), this);
3975         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3976         assertFalse(calibrator.isReady());
3977         assertFalse(calibrator.isRunning());
3978         assertNull(calibrator.getEstimatedBiases());
3979         assertFalse(calibrator.getEstimatedBiases(null));
3980         assertNull(calibrator.getEstimatedBiasesAsMatrix());
3981         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3982         assertNull(calibrator.getEstimatedBiasFx());
3983         assertNull(calibrator.getEstimatedBiasFy());
3984         assertNull(calibrator.getEstimatedBiasFz());
3985         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3986         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3987         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3988         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3989         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3990         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3991         assertNull(calibrator.getEstimatedMa());
3992         assertNull(calibrator.getEstimatedSx());
3993         assertNull(calibrator.getEstimatedSy());
3994         assertNull(calibrator.getEstimatedSz());
3995         assertNull(calibrator.getEstimatedMxy());
3996         assertNull(calibrator.getEstimatedMxz());
3997         assertNull(calibrator.getEstimatedMyx());
3998         assertNull(calibrator.getEstimatedMyz());
3999         assertNull(calibrator.getEstimatedMzx());
4000         assertNull(calibrator.getEstimatedMzy());
4001         assertNull(calibrator.getEstimatedCovariance());
4002         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4003         assertNull(calibrator.getGroundTruthGravityNorm());
4004         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4005         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4006     }
4007 
4008     @Test
4009     public void testConstructor38() throws WrongSizeException {
4010         final Collection<StandardDeviationBodyKinematics> measurements =
4011                 Collections.emptyList();
4012 
4013         final Matrix ba = generateBa();
4014         final double biasX = ba.getElementAtIndex(0);
4015         final double biasY = ba.getElementAtIndex(1);
4016         final double biasZ = ba.getElementAtIndex(2);
4017 
4018         final Matrix ma = generateMaCommonAxis();
4019         final double sx = ma.getElementAt(0, 0);
4020         final double sy = ma.getElementAt(1, 1);
4021         final double sz = ma.getElementAt(2, 2);
4022 
4023         final Acceleration bx = new Acceleration(biasX,
4024                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4025         final Acceleration by = new Acceleration(biasY,
4026                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4027         final Acceleration bz = new Acceleration(biasZ,
4028                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4029 
4030         final KnownPositionAccelerometerCalibrator calibrator =
4031                 new KnownPositionAccelerometerCalibrator(measurements,
4032                         true, bx, by, bz, sx, sy, sz);
4033 
4034         // check default values
4035         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4036         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4037         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4038         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4039         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4040         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4041         final Acceleration bx2 = new Acceleration(0.0,
4042                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4043         calibrator.getInitialBiasXAsAcceleration(bx2);
4044         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4045         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4046         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4047         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4048         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4049         final Acceleration by2 = new Acceleration(0.0,
4050                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4051         calibrator.getInitialBiasYAsAcceleration(by2);
4052         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4053         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4054         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4055         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4056         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4057         final Acceleration bz2 = new Acceleration(0.0,
4058                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4059         calibrator.getInitialBiasZAsAcceleration(bz2);
4060         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4061         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4062         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4063         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4064         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4065         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
4066         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
4067         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
4068         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
4069         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
4070         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
4071         final double[] bias1 = calibrator.getInitialBias();
4072         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4073         final double[] bias2 = new double[3];
4074         calibrator.getInitialBias(bias2);
4075         assertArrayEquals(bias1, bias2, 0.0);
4076         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4077         assertEquals(b1, ba);
4078         final Matrix b2 = new Matrix(3, 1);
4079         calibrator.getInitialBiasAsMatrix(b2);
4080         assertEquals(b1, b2);
4081         final Matrix ma1 = calibrator.getInitialMa();
4082         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
4083         final Matrix ma2 = new Matrix(3, 3);
4084         calibrator.getInitialMa(ma2);
4085         assertEquals(ma1, ma2);
4086         assertNull(calibrator.getEcefPosition());
4087         assertNull(calibrator.getNedPosition());
4088         assertFalse(calibrator.getNedPosition(null));
4089         assertSame(calibrator.getMeasurements(), measurements);
4090         assertTrue(calibrator.isCommonAxisUsed());
4091         assertNull(calibrator.getListener());
4092         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4093         assertFalse(calibrator.isReady());
4094         assertFalse(calibrator.isRunning());
4095         assertNull(calibrator.getEstimatedBiases());
4096         assertFalse(calibrator.getEstimatedBiases(null));
4097         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4098         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4099         assertNull(calibrator.getEstimatedBiasFx());
4100         assertNull(calibrator.getEstimatedBiasFy());
4101         assertNull(calibrator.getEstimatedBiasFz());
4102         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4103         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4104         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4105         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4106         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4107         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4108         assertNull(calibrator.getEstimatedMa());
4109         assertNull(calibrator.getEstimatedSx());
4110         assertNull(calibrator.getEstimatedSy());
4111         assertNull(calibrator.getEstimatedSz());
4112         assertNull(calibrator.getEstimatedMxy());
4113         assertNull(calibrator.getEstimatedMxz());
4114         assertNull(calibrator.getEstimatedMyx());
4115         assertNull(calibrator.getEstimatedMyz());
4116         assertNull(calibrator.getEstimatedMzx());
4117         assertNull(calibrator.getEstimatedMzy());
4118         assertNull(calibrator.getEstimatedCovariance());
4119         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4120         assertNull(calibrator.getGroundTruthGravityNorm());
4121         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4122         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4123     }
4124 
4125     @Test
4126     public void testConstructor39() throws WrongSizeException {
4127         final Collection<StandardDeviationBodyKinematics> measurements =
4128                 Collections.emptyList();
4129 
4130         final Matrix ba = generateBa();
4131         final double biasX = ba.getElementAtIndex(0);
4132         final double biasY = ba.getElementAtIndex(1);
4133         final double biasZ = ba.getElementAtIndex(2);
4134 
4135         final Matrix ma = generateMaCommonAxis();
4136         final double sx = ma.getElementAt(0, 0);
4137         final double sy = ma.getElementAt(1, 1);
4138         final double sz = ma.getElementAt(2, 2);
4139 
4140         final Acceleration bx = new Acceleration(biasX,
4141                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4142         final Acceleration by = new Acceleration(biasY,
4143                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4144         final Acceleration bz = new Acceleration(biasZ,
4145                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4146 
4147         final KnownPositionAccelerometerCalibrator calibrator =
4148                 new KnownPositionAccelerometerCalibrator(measurements,
4149                         true, bx, by, bz, sx, sy, sz,
4150                         this);
4151 
4152         // check default values
4153         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4154         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4155         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4156         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4157         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4158         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4159         final Acceleration bx2 = new Acceleration(0.0,
4160                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4161         calibrator.getInitialBiasXAsAcceleration(bx2);
4162         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4163         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4164         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4165         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4166         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4167         final Acceleration by2 = new Acceleration(0.0,
4168                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4169         calibrator.getInitialBiasYAsAcceleration(by2);
4170         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4171         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4172         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4173         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4174         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4175         final Acceleration bz2 = new Acceleration(0.0,
4176                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4177         calibrator.getInitialBiasZAsAcceleration(bz2);
4178         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4179         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4180         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4181         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4182         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4183         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
4184         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
4185         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
4186         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
4187         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
4188         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
4189         final double[] bias1 = calibrator.getInitialBias();
4190         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4191         final double[] bias2 = new double[3];
4192         calibrator.getInitialBias(bias2);
4193         assertArrayEquals(bias1, bias2, 0.0);
4194         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4195         assertEquals(b1, ba);
4196         final Matrix b2 = new Matrix(3, 1);
4197         calibrator.getInitialBiasAsMatrix(b2);
4198         assertEquals(b1, b2);
4199         final Matrix ma1 = calibrator.getInitialMa();
4200         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
4201         final Matrix ma2 = new Matrix(3, 3);
4202         calibrator.getInitialMa(ma2);
4203         assertEquals(ma1, ma2);
4204         assertNull(calibrator.getEcefPosition());
4205         assertNull(calibrator.getNedPosition());
4206         assertFalse(calibrator.getNedPosition(null));
4207         assertSame(calibrator.getMeasurements(), measurements);
4208         assertTrue(calibrator.isCommonAxisUsed());
4209         assertSame(calibrator.getListener(), this);
4210         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4211         assertFalse(calibrator.isReady());
4212         assertFalse(calibrator.isRunning());
4213         assertNull(calibrator.getEstimatedBiases());
4214         assertFalse(calibrator.getEstimatedBiases(null));
4215         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4216         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4217         assertNull(calibrator.getEstimatedBiasFx());
4218         assertNull(calibrator.getEstimatedBiasFy());
4219         assertNull(calibrator.getEstimatedBiasFz());
4220         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4221         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4222         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4223         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4224         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4225         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4226         assertNull(calibrator.getEstimatedMa());
4227         assertNull(calibrator.getEstimatedSx());
4228         assertNull(calibrator.getEstimatedSy());
4229         assertNull(calibrator.getEstimatedSz());
4230         assertNull(calibrator.getEstimatedMxy());
4231         assertNull(calibrator.getEstimatedMxz());
4232         assertNull(calibrator.getEstimatedMyx());
4233         assertNull(calibrator.getEstimatedMyz());
4234         assertNull(calibrator.getEstimatedMzx());
4235         assertNull(calibrator.getEstimatedMzy());
4236         assertNull(calibrator.getEstimatedCovariance());
4237         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4238         assertNull(calibrator.getGroundTruthGravityNorm());
4239         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4240         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4241     }
4242 
4243     @Test
4244     public void testConstructor40() throws WrongSizeException {
4245         final Matrix ba = generateBa();
4246         final double biasX = ba.getElementAtIndex(0);
4247         final double biasY = ba.getElementAtIndex(1);
4248         final double biasZ = ba.getElementAtIndex(2);
4249 
4250         final Matrix ma = generateMaCommonAxis();
4251         final double sx = ma.getElementAt(0, 0);
4252         final double sy = ma.getElementAt(1, 1);
4253         final double sz = ma.getElementAt(2, 2);
4254         final double mxy = ma.getElementAt(0, 1);
4255         final double mxz = ma.getElementAt(0, 2);
4256         final double myx = ma.getElementAt(1, 0);
4257         final double myz = ma.getElementAt(1, 2);
4258         final double mzx = ma.getElementAt(2, 0);
4259         final double mzy = ma.getElementAt(2, 1);
4260 
4261         final KnownPositionAccelerometerCalibrator calibrator =
4262                 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
4263                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4264 
4265         // check default values
4266         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4267         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4268         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4269         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4270         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4271         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4272         final Acceleration bx2 = new Acceleration(0.0,
4273                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4274         calibrator.getInitialBiasXAsAcceleration(bx2);
4275         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4276         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4277         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4278         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4279         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4280         final Acceleration by2 = new Acceleration(0.0,
4281                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4282         calibrator.getInitialBiasYAsAcceleration(by2);
4283         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4284         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4285         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4286         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4287         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4288         final Acceleration bz2 = new Acceleration(0.0,
4289                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4290         calibrator.getInitialBiasZAsAcceleration(bz2);
4291         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4292         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4293         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4294         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4295         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4296         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4297         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4298         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4299         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4300         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4301         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4302         final double[] bias1 = calibrator.getInitialBias();
4303         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4304         final double[] bias2 = new double[3];
4305         calibrator.getInitialBias(bias2);
4306         assertArrayEquals(bias1, bias2, 0.0);
4307         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4308         assertEquals(b1, ba);
4309         final Matrix b2 = new Matrix(3, 1);
4310         calibrator.getInitialBiasAsMatrix(b2);
4311         assertEquals(b1, b2);
4312         final Matrix ma1 = new Matrix(3, 3);
4313         ma1.setSubmatrix(0, 0,
4314                 2, 2,
4315                 new double[]{sx, myx, mzx,
4316                         mxy, sy, mzy,
4317                         mxz, myz, sz});
4318         assertEquals(calibrator.getInitialMa(), ma1);
4319         final Matrix ma2 = new Matrix(3, 3);
4320         calibrator.getInitialMa(ma2);
4321         assertEquals(ma1, ma2);
4322         assertNull(calibrator.getEcefPosition());
4323         assertNull(calibrator.getNedPosition());
4324         assertFalse(calibrator.getNedPosition(null));
4325         assertNull(calibrator.getMeasurements());
4326         assertFalse(calibrator.isCommonAxisUsed());
4327         assertNull(calibrator.getListener());
4328         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4329         assertFalse(calibrator.isReady());
4330         assertFalse(calibrator.isRunning());
4331         assertNull(calibrator.getEstimatedBiases());
4332         assertFalse(calibrator.getEstimatedBiases(null));
4333         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4334         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4335         assertNull(calibrator.getEstimatedBiasFx());
4336         assertNull(calibrator.getEstimatedBiasFy());
4337         assertNull(calibrator.getEstimatedBiasFz());
4338         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4339         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4340         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4341         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4342         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4343         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4344         assertNull(calibrator.getEstimatedMa());
4345         assertNull(calibrator.getEstimatedSx());
4346         assertNull(calibrator.getEstimatedSy());
4347         assertNull(calibrator.getEstimatedSz());
4348         assertNull(calibrator.getEstimatedMxy());
4349         assertNull(calibrator.getEstimatedMxz());
4350         assertNull(calibrator.getEstimatedMyx());
4351         assertNull(calibrator.getEstimatedMyz());
4352         assertNull(calibrator.getEstimatedMzx());
4353         assertNull(calibrator.getEstimatedMzy());
4354         assertNull(calibrator.getEstimatedCovariance());
4355         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4356         assertNull(calibrator.getGroundTruthGravityNorm());
4357         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4358         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4359     }
4360 
4361     @Test
4362     public void testConstructor41() throws WrongSizeException {
4363         final Collection<StandardDeviationBodyKinematics> measurements =
4364                 Collections.emptyList();
4365 
4366         final Matrix ba = generateBa();
4367         final double biasX = ba.getElementAtIndex(0);
4368         final double biasY = ba.getElementAtIndex(1);
4369         final double biasZ = ba.getElementAtIndex(2);
4370 
4371         final Matrix ma = generateMaCommonAxis();
4372         final double sx = ma.getElementAt(0, 0);
4373         final double sy = ma.getElementAt(1, 1);
4374         final double sz = ma.getElementAt(2, 2);
4375         final double mxy = ma.getElementAt(0, 1);
4376         final double mxz = ma.getElementAt(0, 2);
4377         final double myx = ma.getElementAt(1, 0);
4378         final double myz = ma.getElementAt(1, 2);
4379         final double mzx = ma.getElementAt(2, 0);
4380         final double mzy = ma.getElementAt(2, 1);
4381 
4382         final KnownPositionAccelerometerCalibrator calibrator =
4383                 new KnownPositionAccelerometerCalibrator(measurements,
4384                         biasX, biasY, biasZ, sx, sy, sz,
4385                         mxy, mxz, myx, myz, mzx, mzy);
4386 
4387         // check default values
4388         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4389         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4390         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4391         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4392         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4393         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4394         final Acceleration bx2 = new Acceleration(0.0,
4395                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4396         calibrator.getInitialBiasXAsAcceleration(bx2);
4397         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4398         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4399         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4400         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4401         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4402         final Acceleration by2 = new Acceleration(0.0,
4403                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4404         calibrator.getInitialBiasYAsAcceleration(by2);
4405         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4406         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4407         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4408         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4409         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4410         final Acceleration bz2 = new Acceleration(0.0,
4411                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4412         calibrator.getInitialBiasZAsAcceleration(bz2);
4413         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4414         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4415         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4416         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4417         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4418         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4419         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4420         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4421         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4422         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4423         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4424         final double[] bias1 = calibrator.getInitialBias();
4425         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4426         final double[] bias2 = new double[3];
4427         calibrator.getInitialBias(bias2);
4428         assertArrayEquals(bias1, bias2, 0.0);
4429         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4430         assertEquals(b1, ba);
4431         final Matrix b2 = new Matrix(3, 1);
4432         calibrator.getInitialBiasAsMatrix(b2);
4433         assertEquals(b1, b2);
4434         final Matrix ma1 = new Matrix(3, 3);
4435         ma1.setSubmatrix(0, 0,
4436                 2, 2,
4437                 new double[]{sx, myx, mzx,
4438                         mxy, sy, mzy,
4439                         mxz, myz, sz});
4440         assertEquals(calibrator.getInitialMa(), ma1);
4441         final Matrix ma2 = new Matrix(3, 3);
4442         calibrator.getInitialMa(ma2);
4443         assertEquals(ma1, ma2);
4444         assertNull(calibrator.getEcefPosition());
4445         assertNull(calibrator.getNedPosition());
4446         assertFalse(calibrator.getNedPosition(null));
4447         assertSame(calibrator.getMeasurements(), measurements);
4448         assertFalse(calibrator.isCommonAxisUsed());
4449         assertNull(calibrator.getListener());
4450         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4451         assertFalse(calibrator.isReady());
4452         assertFalse(calibrator.isRunning());
4453         assertNull(calibrator.getEstimatedBiases());
4454         assertFalse(calibrator.getEstimatedBiases(null));
4455         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4456         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4457         assertNull(calibrator.getEstimatedBiasFx());
4458         assertNull(calibrator.getEstimatedBiasFy());
4459         assertNull(calibrator.getEstimatedBiasFz());
4460         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4461         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4462         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4463         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4464         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4465         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4466         assertNull(calibrator.getEstimatedMa());
4467         assertNull(calibrator.getEstimatedSx());
4468         assertNull(calibrator.getEstimatedSy());
4469         assertNull(calibrator.getEstimatedSz());
4470         assertNull(calibrator.getEstimatedMxy());
4471         assertNull(calibrator.getEstimatedMxz());
4472         assertNull(calibrator.getEstimatedMyx());
4473         assertNull(calibrator.getEstimatedMyz());
4474         assertNull(calibrator.getEstimatedMzx());
4475         assertNull(calibrator.getEstimatedMzy());
4476         assertNull(calibrator.getEstimatedCovariance());
4477         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4478         assertNull(calibrator.getGroundTruthGravityNorm());
4479         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4480         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4481     }
4482 
4483     @Test
4484     public void testConstructor42() throws WrongSizeException {
4485         final Collection<StandardDeviationBodyKinematics> measurements =
4486                 Collections.emptyList();
4487 
4488         final Matrix ba = generateBa();
4489         final double biasX = ba.getElementAtIndex(0);
4490         final double biasY = ba.getElementAtIndex(1);
4491         final double biasZ = ba.getElementAtIndex(2);
4492 
4493         final Matrix ma = generateMaCommonAxis();
4494         final double sx = ma.getElementAt(0, 0);
4495         final double sy = ma.getElementAt(1, 1);
4496         final double sz = ma.getElementAt(2, 2);
4497         final double mxy = ma.getElementAt(0, 1);
4498         final double mxz = ma.getElementAt(0, 2);
4499         final double myx = ma.getElementAt(1, 0);
4500         final double myz = ma.getElementAt(1, 2);
4501         final double mzx = ma.getElementAt(2, 0);
4502         final double mzy = ma.getElementAt(2, 1);
4503 
4504         final KnownPositionAccelerometerCalibrator calibrator =
4505                 new KnownPositionAccelerometerCalibrator(measurements,
4506                         biasX, biasY, biasZ, sx, sy, sz,
4507                         mxy, mxz, myx, myz, mzx, mzy, this);
4508 
4509         // check default values
4510         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4511         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4512         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4513         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4514         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4515         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4516         final Acceleration bx2 = new Acceleration(0.0,
4517                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4518         calibrator.getInitialBiasXAsAcceleration(bx2);
4519         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4520         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4521         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4522         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4523         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4524         final Acceleration by2 = new Acceleration(0.0,
4525                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4526         calibrator.getInitialBiasYAsAcceleration(by2);
4527         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4528         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4529         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4530         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4531         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4532         final Acceleration bz2 = new Acceleration(0.0,
4533                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4534         calibrator.getInitialBiasZAsAcceleration(bz2);
4535         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4536         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4537         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4538         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4539         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4540         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4541         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4542         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4543         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4544         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4545         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4546         final double[] bias1 = calibrator.getInitialBias();
4547         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4548         final double[] bias2 = new double[3];
4549         calibrator.getInitialBias(bias2);
4550         assertArrayEquals(bias1, bias2, 0.0);
4551         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4552         assertEquals(b1, ba);
4553         final Matrix b2 = new Matrix(3, 1);
4554         calibrator.getInitialBiasAsMatrix(b2);
4555         assertEquals(b1, b2);
4556         final Matrix ma1 = new Matrix(3, 3);
4557         ma1.setSubmatrix(0, 0,
4558                 2, 2,
4559                 new double[]{sx, myx, mzx,
4560                         mxy, sy, mzy,
4561                         mxz, myz, sz});
4562         assertEquals(calibrator.getInitialMa(), ma1);
4563         final Matrix ma2 = new Matrix(3, 3);
4564         calibrator.getInitialMa(ma2);
4565         assertEquals(ma1, ma2);
4566         assertNull(calibrator.getEcefPosition());
4567         assertNull(calibrator.getNedPosition());
4568         assertFalse(calibrator.getNedPosition(null));
4569         assertSame(calibrator.getMeasurements(), measurements);
4570         assertFalse(calibrator.isCommonAxisUsed());
4571         assertSame(calibrator.getListener(), this);
4572         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4573         assertFalse(calibrator.isReady());
4574         assertFalse(calibrator.isRunning());
4575         assertNull(calibrator.getEstimatedBiases());
4576         assertFalse(calibrator.getEstimatedBiases(null));
4577         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4578         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4579         assertNull(calibrator.getEstimatedBiasFx());
4580         assertNull(calibrator.getEstimatedBiasFy());
4581         assertNull(calibrator.getEstimatedBiasFz());
4582         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4583         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4584         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4585         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4586         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4587         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4588         assertNull(calibrator.getEstimatedMa());
4589         assertNull(calibrator.getEstimatedSx());
4590         assertNull(calibrator.getEstimatedSy());
4591         assertNull(calibrator.getEstimatedSz());
4592         assertNull(calibrator.getEstimatedMxy());
4593         assertNull(calibrator.getEstimatedMxz());
4594         assertNull(calibrator.getEstimatedMyx());
4595         assertNull(calibrator.getEstimatedMyz());
4596         assertNull(calibrator.getEstimatedMzx());
4597         assertNull(calibrator.getEstimatedMzy());
4598         assertNull(calibrator.getEstimatedCovariance());
4599         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4600         assertNull(calibrator.getGroundTruthGravityNorm());
4601         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4602         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4603     }
4604 
4605     @Test
4606     public void testConstructor43() throws WrongSizeException {
4607         final Matrix ba = generateBa();
4608         final double biasX = ba.getElementAtIndex(0);
4609         final double biasY = ba.getElementAtIndex(1);
4610         final double biasZ = ba.getElementAtIndex(2);
4611 
4612         final Matrix ma = generateMaCommonAxis();
4613         final double sx = ma.getElementAt(0, 0);
4614         final double sy = ma.getElementAt(1, 1);
4615         final double sz = ma.getElementAt(2, 2);
4616         final double mxy = ma.getElementAt(0, 1);
4617         final double mxz = ma.getElementAt(0, 2);
4618         final double myx = ma.getElementAt(1, 0);
4619         final double myz = ma.getElementAt(1, 2);
4620         final double mzx = ma.getElementAt(2, 0);
4621         final double mzy = ma.getElementAt(2, 1);
4622 
4623         final KnownPositionAccelerometerCalibrator calibrator =
4624                 new KnownPositionAccelerometerCalibrator(true,
4625                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4626                         myz, mzx, mzy);
4627 
4628         // check default values
4629         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4630         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4631         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4632         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4633         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4634         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4635         final Acceleration bx2 = new Acceleration(0.0,
4636                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4637         calibrator.getInitialBiasXAsAcceleration(bx2);
4638         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4639         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4640         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4641         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4642         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4643         final Acceleration by2 = new Acceleration(0.0,
4644                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4645         calibrator.getInitialBiasYAsAcceleration(by2);
4646         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4647         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4648         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4649         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4650         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4651         final Acceleration bz2 = new Acceleration(0.0,
4652                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4653         calibrator.getInitialBiasZAsAcceleration(bz2);
4654         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4655         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4656         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4657         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4658         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4659         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4660         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4661         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4662         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4663         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4664         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4665         final double[] bias1 = calibrator.getInitialBias();
4666         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4667         final double[] bias2 = new double[3];
4668         calibrator.getInitialBias(bias2);
4669         assertArrayEquals(bias1, bias2, 0.0);
4670         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4671         assertEquals(b1, ba);
4672         final Matrix b2 = new Matrix(3, 1);
4673         calibrator.getInitialBiasAsMatrix(b2);
4674         assertEquals(b1, b2);
4675         final Matrix ma1 = new Matrix(3, 3);
4676         ma1.setSubmatrix(0, 0,
4677                 2, 2,
4678                 new double[]{sx, myx, mzx,
4679                         mxy, sy, mzy,
4680                         mxz, myz, sz});
4681         assertEquals(calibrator.getInitialMa(), ma1);
4682         final Matrix ma2 = new Matrix(3, 3);
4683         calibrator.getInitialMa(ma2);
4684         assertEquals(ma1, ma2);
4685         assertNull(calibrator.getEcefPosition());
4686         assertNull(calibrator.getNedPosition());
4687         assertFalse(calibrator.getNedPosition(null));
4688         assertNull(calibrator.getMeasurements());
4689         assertTrue(calibrator.isCommonAxisUsed());
4690         assertNull(calibrator.getListener());
4691         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4692         assertFalse(calibrator.isReady());
4693         assertFalse(calibrator.isRunning());
4694         assertNull(calibrator.getEstimatedBiases());
4695         assertFalse(calibrator.getEstimatedBiases(null));
4696         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4697         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4698         assertNull(calibrator.getEstimatedBiasFx());
4699         assertNull(calibrator.getEstimatedBiasFy());
4700         assertNull(calibrator.getEstimatedBiasFz());
4701         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4702         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4703         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4704         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4705         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4706         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4707         assertNull(calibrator.getEstimatedMa());
4708         assertNull(calibrator.getEstimatedSx());
4709         assertNull(calibrator.getEstimatedSy());
4710         assertNull(calibrator.getEstimatedSz());
4711         assertNull(calibrator.getEstimatedMxy());
4712         assertNull(calibrator.getEstimatedMxz());
4713         assertNull(calibrator.getEstimatedMyx());
4714         assertNull(calibrator.getEstimatedMyz());
4715         assertNull(calibrator.getEstimatedMzx());
4716         assertNull(calibrator.getEstimatedMzy());
4717         assertNull(calibrator.getEstimatedCovariance());
4718         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4719         assertNull(calibrator.getGroundTruthGravityNorm());
4720         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4721         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4722     }
4723 
4724     @Test
4725     public void testConstructor44() throws WrongSizeException {
4726         final Matrix ba = generateBa();
4727         final double biasX = ba.getElementAtIndex(0);
4728         final double biasY = ba.getElementAtIndex(1);
4729         final double biasZ = ba.getElementAtIndex(2);
4730 
4731         final Matrix ma = generateMaCommonAxis();
4732         final double sx = ma.getElementAt(0, 0);
4733         final double sy = ma.getElementAt(1, 1);
4734         final double sz = ma.getElementAt(2, 2);
4735         final double mxy = ma.getElementAt(0, 1);
4736         final double mxz = ma.getElementAt(0, 2);
4737         final double myx = ma.getElementAt(1, 0);
4738         final double myz = ma.getElementAt(1, 2);
4739         final double mzx = ma.getElementAt(2, 0);
4740         final double mzy = ma.getElementAt(2, 1);
4741 
4742         final KnownPositionAccelerometerCalibrator calibrator =
4743                 new KnownPositionAccelerometerCalibrator(true,
4744                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4745                         myz, mzx, mzy, this);
4746 
4747         // check default values
4748         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4749         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4750         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4751         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4752         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4753         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4754         final Acceleration bx2 = new Acceleration(0.0,
4755                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4756         calibrator.getInitialBiasXAsAcceleration(bx2);
4757         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4758         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4759         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4760         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4761         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4762         final Acceleration by2 = new Acceleration(0.0,
4763                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4764         calibrator.getInitialBiasYAsAcceleration(by2);
4765         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4766         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4767         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4768         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4769         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4770         final Acceleration bz2 = new Acceleration(0.0,
4771                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4772         calibrator.getInitialBiasZAsAcceleration(bz2);
4773         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4774         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4775         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4776         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4777         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4778         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4779         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4780         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4781         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4782         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4783         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4784         final double[] bias1 = calibrator.getInitialBias();
4785         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4786         final double[] bias2 = new double[3];
4787         calibrator.getInitialBias(bias2);
4788         assertArrayEquals(bias1, bias2, 0.0);
4789         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4790         assertEquals(b1, ba);
4791         final Matrix b2 = new Matrix(3, 1);
4792         calibrator.getInitialBiasAsMatrix(b2);
4793         assertEquals(b1, b2);
4794         final Matrix ma1 = new Matrix(3, 3);
4795         ma1.setSubmatrix(0, 0,
4796                 2, 2,
4797                 new double[]{sx, myx, mzx,
4798                         mxy, sy, mzy,
4799                         mxz, myz, sz});
4800         assertEquals(calibrator.getInitialMa(), ma1);
4801         final Matrix ma2 = new Matrix(3, 3);
4802         calibrator.getInitialMa(ma2);
4803         assertEquals(ma1, ma2);
4804         assertNull(calibrator.getEcefPosition());
4805         assertNull(calibrator.getNedPosition());
4806         assertFalse(calibrator.getNedPosition(null));
4807         assertNull(calibrator.getMeasurements());
4808         assertTrue(calibrator.isCommonAxisUsed());
4809         assertSame(calibrator.getListener(), this);
4810         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4811         assertFalse(calibrator.isReady());
4812         assertFalse(calibrator.isRunning());
4813         assertNull(calibrator.getEstimatedBiases());
4814         assertFalse(calibrator.getEstimatedBiases(null));
4815         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4816         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4817         assertNull(calibrator.getEstimatedBiasFx());
4818         assertNull(calibrator.getEstimatedBiasFy());
4819         assertNull(calibrator.getEstimatedBiasFz());
4820         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4821         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4822         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4823         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4824         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4825         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4826         assertNull(calibrator.getEstimatedMa());
4827         assertNull(calibrator.getEstimatedSx());
4828         assertNull(calibrator.getEstimatedSy());
4829         assertNull(calibrator.getEstimatedSz());
4830         assertNull(calibrator.getEstimatedMxy());
4831         assertNull(calibrator.getEstimatedMxz());
4832         assertNull(calibrator.getEstimatedMyx());
4833         assertNull(calibrator.getEstimatedMyz());
4834         assertNull(calibrator.getEstimatedMzx());
4835         assertNull(calibrator.getEstimatedMzy());
4836         assertNull(calibrator.getEstimatedCovariance());
4837         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4838         assertNull(calibrator.getGroundTruthGravityNorm());
4839         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4840         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4841     }
4842 
4843     @Test
4844     public void testConstructor45() throws WrongSizeException {
4845         final Collection<StandardDeviationBodyKinematics> measurements =
4846                 Collections.emptyList();
4847 
4848         final Matrix ba = generateBa();
4849         final double biasX = ba.getElementAtIndex(0);
4850         final double biasY = ba.getElementAtIndex(1);
4851         final double biasZ = ba.getElementAtIndex(2);
4852 
4853         final Matrix ma = generateMaCommonAxis();
4854         final double sx = ma.getElementAt(0, 0);
4855         final double sy = ma.getElementAt(1, 1);
4856         final double sz = ma.getElementAt(2, 2);
4857         final double mxy = ma.getElementAt(0, 1);
4858         final double mxz = ma.getElementAt(0, 2);
4859         final double myx = ma.getElementAt(1, 0);
4860         final double myz = ma.getElementAt(1, 2);
4861         final double mzx = ma.getElementAt(2, 0);
4862         final double mzy = ma.getElementAt(2, 1);
4863 
4864         final KnownPositionAccelerometerCalibrator calibrator =
4865                 new KnownPositionAccelerometerCalibrator(measurements,
4866                         true, biasX, biasY, biasZ,
4867                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4868 
4869         // check default values
4870         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4871         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4872         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4873         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4874         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4875         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4876         final Acceleration bx2 = new Acceleration(0.0,
4877                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4878         calibrator.getInitialBiasXAsAcceleration(bx2);
4879         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4880         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4881         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4882         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4883         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4884         final Acceleration by2 = new Acceleration(0.0,
4885                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4886         calibrator.getInitialBiasYAsAcceleration(by2);
4887         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4888         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4889         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4890         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4891         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4892         final Acceleration bz2 = new Acceleration(0.0,
4893                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4894         calibrator.getInitialBiasZAsAcceleration(bz2);
4895         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4896         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4897         assertEquals(calibrator.getInitialSx(), sx, 0.0);
4898         assertEquals(calibrator.getInitialSy(), sy, 0.0);
4899         assertEquals(calibrator.getInitialSz(), sz, 0.0);
4900         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4901         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4902         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4903         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4904         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4905         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4906         final double[] bias1 = calibrator.getInitialBias();
4907         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4908         final double[] bias2 = new double[3];
4909         calibrator.getInitialBias(bias2);
4910         assertArrayEquals(bias1, bias2, 0.0);
4911         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4912         assertEquals(b1, ba);
4913         final Matrix b2 = new Matrix(3, 1);
4914         calibrator.getInitialBiasAsMatrix(b2);
4915         assertEquals(b1, b2);
4916         final Matrix ma1 = new Matrix(3, 3);
4917         ma1.setSubmatrix(0, 0,
4918                 2, 2,
4919                 new double[]{sx, myx, mzx,
4920                         mxy, sy, mzy,
4921                         mxz, myz, sz});
4922         assertEquals(calibrator.getInitialMa(), ma1);
4923         final Matrix ma2 = new Matrix(3, 3);
4924         calibrator.getInitialMa(ma2);
4925         assertEquals(ma1, ma2);
4926         assertNull(calibrator.getEcefPosition());
4927         assertNull(calibrator.getNedPosition());
4928         assertFalse(calibrator.getNedPosition(null));
4929         assertSame(calibrator.getMeasurements(), measurements);
4930         assertTrue(calibrator.isCommonAxisUsed());
4931         assertNull(calibrator.getListener());
4932         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4933         assertFalse(calibrator.isReady());
4934         assertFalse(calibrator.isRunning());
4935         assertNull(calibrator.getEstimatedBiases());
4936         assertFalse(calibrator.getEstimatedBiases(null));
4937         assertNull(calibrator.getEstimatedBiasesAsMatrix());
4938         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4939         assertNull(calibrator.getEstimatedBiasFx());
4940         assertNull(calibrator.getEstimatedBiasFy());
4941         assertNull(calibrator.getEstimatedBiasFz());
4942         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4943         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4944         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4945         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4946         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4947         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4948         assertNull(calibrator.getEstimatedMa());
4949         assertNull(calibrator.getEstimatedSx());
4950         assertNull(calibrator.getEstimatedSy());
4951         assertNull(calibrator.getEstimatedSz());
4952         assertNull(calibrator.getEstimatedMxy());
4953         assertNull(calibrator.getEstimatedMxz());
4954         assertNull(calibrator.getEstimatedMyx());
4955         assertNull(calibrator.getEstimatedMyz());
4956         assertNull(calibrator.getEstimatedMzx());
4957         assertNull(calibrator.getEstimatedMzy());
4958         assertNull(calibrator.getEstimatedCovariance());
4959         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4960         assertNull(calibrator.getGroundTruthGravityNorm());
4961         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4962         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4963     }
4964 
4965     @Test
4966     public void testConstructor46() throws WrongSizeException {
4967         final Collection<StandardDeviationBodyKinematics> measurements =
4968                 Collections.emptyList();
4969 
4970         final Matrix ba = generateBa();
4971         final double biasX = ba.getElementAtIndex(0);
4972         final double biasY = ba.getElementAtIndex(1);
4973         final double biasZ = ba.getElementAtIndex(2);
4974 
4975         final Matrix ma = generateMaCommonAxis();
4976         final double sx = ma.getElementAt(0, 0);
4977         final double sy = ma.getElementAt(1, 1);
4978         final double sz = ma.getElementAt(2, 2);
4979         final double mxy = ma.getElementAt(0, 1);
4980         final double mxz = ma.getElementAt(0, 2);
4981         final double myx = ma.getElementAt(1, 0);
4982         final double myz = ma.getElementAt(1, 2);
4983         final double mzx = ma.getElementAt(2, 0);
4984         final double mzy = ma.getElementAt(2, 1);
4985 
4986         final KnownPositionAccelerometerCalibrator calibrator =
4987                 new KnownPositionAccelerometerCalibrator(measurements,
4988                         true, biasX, biasY, biasZ,
4989                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4990 
4991         // check default values
4992         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4993         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4994         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4995         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4996         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4997         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4998         final Acceleration bx2 = new Acceleration(0.0,
4999                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5000         calibrator.getInitialBiasXAsAcceleration(bx2);
5001         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5002         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5003         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5004         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5005         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5006         final Acceleration by2 = new Acceleration(0.0,
5007                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5008         calibrator.getInitialBiasYAsAcceleration(by2);
5009         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5010         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5011         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5012         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5013         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5014         final Acceleration bz2 = new Acceleration(0.0,
5015                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5016         calibrator.getInitialBiasZAsAcceleration(bz2);
5017         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5018         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5019         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5020         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5021         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5022         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5023         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5024         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5025         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5026         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5027         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5028         final double[] bias1 = calibrator.getInitialBias();
5029         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5030         final double[] bias2 = new double[3];
5031         calibrator.getInitialBias(bias2);
5032         assertArrayEquals(bias1, bias2, 0.0);
5033         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5034         assertEquals(b1, ba);
5035         final Matrix b2 = new Matrix(3, 1);
5036         calibrator.getInitialBiasAsMatrix(b2);
5037         assertEquals(b1, b2);
5038         final Matrix ma1 = new Matrix(3, 3);
5039         ma1.setSubmatrix(0, 0,
5040                 2, 2,
5041                 new double[]{sx, myx, mzx,
5042                         mxy, sy, mzy,
5043                         mxz, myz, sz});
5044         assertEquals(calibrator.getInitialMa(), ma1);
5045         final Matrix ma2 = new Matrix(3, 3);
5046         calibrator.getInitialMa(ma2);
5047         assertEquals(ma1, ma2);
5048         assertNull(calibrator.getEcefPosition());
5049         assertNull(calibrator.getNedPosition());
5050         assertFalse(calibrator.getNedPosition(null));
5051         assertSame(calibrator.getMeasurements(), measurements);
5052         assertTrue(calibrator.isCommonAxisUsed());
5053         assertSame(calibrator.getListener(), this);
5054         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5055         assertFalse(calibrator.isReady());
5056         assertFalse(calibrator.isRunning());
5057         assertNull(calibrator.getEstimatedBiases());
5058         assertFalse(calibrator.getEstimatedBiases(null));
5059         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5060         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5061         assertNull(calibrator.getEstimatedBiasFx());
5062         assertNull(calibrator.getEstimatedBiasFy());
5063         assertNull(calibrator.getEstimatedBiasFz());
5064         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5065         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5066         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5067         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5068         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5069         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5070         assertNull(calibrator.getEstimatedMa());
5071         assertNull(calibrator.getEstimatedSx());
5072         assertNull(calibrator.getEstimatedSy());
5073         assertNull(calibrator.getEstimatedSz());
5074         assertNull(calibrator.getEstimatedMxy());
5075         assertNull(calibrator.getEstimatedMxz());
5076         assertNull(calibrator.getEstimatedMyx());
5077         assertNull(calibrator.getEstimatedMyz());
5078         assertNull(calibrator.getEstimatedMzx());
5079         assertNull(calibrator.getEstimatedMzy());
5080         assertNull(calibrator.getEstimatedCovariance());
5081         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5082         assertNull(calibrator.getGroundTruthGravityNorm());
5083         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5084         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5085     }
5086 
5087     @Test
5088     public void testConstructor47() throws WrongSizeException {
5089         final Matrix ba = generateBa();
5090         final double biasX = ba.getElementAtIndex(0);
5091         final double biasY = ba.getElementAtIndex(1);
5092         final double biasZ = ba.getElementAtIndex(2);
5093 
5094         final Matrix ma = generateMaCommonAxis();
5095         final double sx = ma.getElementAt(0, 0);
5096         final double sy = ma.getElementAt(1, 1);
5097         final double sz = ma.getElementAt(2, 2);
5098         final double mxy = ma.getElementAt(0, 1);
5099         final double mxz = ma.getElementAt(0, 2);
5100         final double myx = ma.getElementAt(1, 0);
5101         final double myz = ma.getElementAt(1, 2);
5102         final double mzx = ma.getElementAt(2, 0);
5103         final double mzy = ma.getElementAt(2, 1);
5104 
5105         final Acceleration bx = new Acceleration(biasX,
5106                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5107         final Acceleration by = new Acceleration(biasY,
5108                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5109         final Acceleration bz = new Acceleration(biasZ,
5110                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5111 
5112         final KnownPositionAccelerometerCalibrator calibrator =
5113                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
5114                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5115 
5116         // check default values
5117         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5118         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5119         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5120         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5121         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5122         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5123         final Acceleration bx2 = new Acceleration(0.0,
5124                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5125         calibrator.getInitialBiasXAsAcceleration(bx2);
5126         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5127         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5128         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5129         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5130         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5131         final Acceleration by2 = new Acceleration(0.0,
5132                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5133         calibrator.getInitialBiasYAsAcceleration(by2);
5134         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5135         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5136         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5137         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5138         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5139         final Acceleration bz2 = new Acceleration(0.0,
5140                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5141         calibrator.getInitialBiasZAsAcceleration(bz2);
5142         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5143         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5144         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5145         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5146         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5147         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5148         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5149         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5150         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5151         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5152         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5153         final double[] bias1 = calibrator.getInitialBias();
5154         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5155         final double[] bias2 = new double[3];
5156         calibrator.getInitialBias(bias2);
5157         assertArrayEquals(bias1, bias2, 0.0);
5158         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5159         assertEquals(b1, ba);
5160         final Matrix b2 = new Matrix(3, 1);
5161         calibrator.getInitialBiasAsMatrix(b2);
5162         assertEquals(b1, b2);
5163         final Matrix ma1 = new Matrix(3, 3);
5164         ma1.setSubmatrix(0, 0,
5165                 2, 2,
5166                 new double[]{sx, myx, mzx,
5167                         mxy, sy, mzy,
5168                         mxz, myz, sz});
5169         assertEquals(calibrator.getInitialMa(), ma1);
5170         final Matrix ma2 = new Matrix(3, 3);
5171         calibrator.getInitialMa(ma2);
5172         assertEquals(ma1, ma2);
5173         assertNull(calibrator.getEcefPosition());
5174         assertNull(calibrator.getNedPosition());
5175         assertFalse(calibrator.getNedPosition(null));
5176         assertNull(calibrator.getMeasurements());
5177         assertFalse(calibrator.isCommonAxisUsed());
5178         assertNull(calibrator.getListener());
5179         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5180         assertFalse(calibrator.isReady());
5181         assertFalse(calibrator.isRunning());
5182         assertNull(calibrator.getEstimatedBiases());
5183         assertFalse(calibrator.getEstimatedBiases(null));
5184         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5185         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5186         assertNull(calibrator.getEstimatedBiasFx());
5187         assertNull(calibrator.getEstimatedBiasFy());
5188         assertNull(calibrator.getEstimatedBiasFz());
5189         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5190         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5191         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5192         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5193         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5194         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5195         assertNull(calibrator.getEstimatedMa());
5196         assertNull(calibrator.getEstimatedSx());
5197         assertNull(calibrator.getEstimatedSy());
5198         assertNull(calibrator.getEstimatedSz());
5199         assertNull(calibrator.getEstimatedMxy());
5200         assertNull(calibrator.getEstimatedMxz());
5201         assertNull(calibrator.getEstimatedMyx());
5202         assertNull(calibrator.getEstimatedMyz());
5203         assertNull(calibrator.getEstimatedMzx());
5204         assertNull(calibrator.getEstimatedMzy());
5205         assertNull(calibrator.getEstimatedCovariance());
5206         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5207         assertNull(calibrator.getGroundTruthGravityNorm());
5208         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5209         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5210     }
5211 
5212     @Test
5213     public void testConstructor48() throws WrongSizeException {
5214         final Matrix ba = generateBa();
5215         final double biasX = ba.getElementAtIndex(0);
5216         final double biasY = ba.getElementAtIndex(1);
5217         final double biasZ = ba.getElementAtIndex(2);
5218 
5219         final Matrix ma = generateMaCommonAxis();
5220         final double sx = ma.getElementAt(0, 0);
5221         final double sy = ma.getElementAt(1, 1);
5222         final double sz = ma.getElementAt(2, 2);
5223         final double mxy = ma.getElementAt(0, 1);
5224         final double mxz = ma.getElementAt(0, 2);
5225         final double myx = ma.getElementAt(1, 0);
5226         final double myz = ma.getElementAt(1, 2);
5227         final double mzx = ma.getElementAt(2, 0);
5228         final double mzy = ma.getElementAt(2, 1);
5229 
5230         final Acceleration bx = new Acceleration(biasX,
5231                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5232         final Acceleration by = new Acceleration(biasY,
5233                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5234         final Acceleration bz = new Acceleration(biasZ,
5235                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5236 
5237         final KnownPositionAccelerometerCalibrator calibrator =
5238                 new KnownPositionAccelerometerCalibrator(bx, by, bz,
5239                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5240 
5241         // check default values
5242         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5243         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5244         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5245         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5246         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5247         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5248         final Acceleration bx2 = new Acceleration(0.0,
5249                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5250         calibrator.getInitialBiasXAsAcceleration(bx2);
5251         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5252         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5253         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5254         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5255         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5256         final Acceleration by2 = new Acceleration(0.0,
5257                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5258         calibrator.getInitialBiasYAsAcceleration(by2);
5259         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5260         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5261         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5262         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5263         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5264         final Acceleration bz2 = new Acceleration(0.0,
5265                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5266         calibrator.getInitialBiasZAsAcceleration(bz2);
5267         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5268         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5269         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5270         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5271         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5272         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5273         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5274         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5275         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5276         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5277         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5278         final double[] bias1 = calibrator.getInitialBias();
5279         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5280         final double[] bias2 = new double[3];
5281         calibrator.getInitialBias(bias2);
5282         assertArrayEquals(bias1, bias2, 0.0);
5283         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5284         assertEquals(b1, ba);
5285         final Matrix b2 = new Matrix(3, 1);
5286         calibrator.getInitialBiasAsMatrix(b2);
5287         assertEquals(b1, b2);
5288         final Matrix ma1 = new Matrix(3, 3);
5289         ma1.setSubmatrix(0, 0,
5290                 2, 2,
5291                 new double[]{sx, myx, mzx,
5292                         mxy, sy, mzy,
5293                         mxz, myz, sz});
5294         assertEquals(calibrator.getInitialMa(), ma1);
5295         final Matrix ma2 = new Matrix(3, 3);
5296         calibrator.getInitialMa(ma2);
5297         assertEquals(ma1, ma2);
5298         assertNull(calibrator.getEcefPosition());
5299         assertNull(calibrator.getNedPosition());
5300         assertFalse(calibrator.getNedPosition(null));
5301         assertNull(calibrator.getMeasurements());
5302         assertFalse(calibrator.isCommonAxisUsed());
5303         assertSame(calibrator.getListener(), this);
5304         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5305         assertFalse(calibrator.isReady());
5306         assertFalse(calibrator.isRunning());
5307         assertNull(calibrator.getEstimatedBiases());
5308         assertFalse(calibrator.getEstimatedBiases(null));
5309         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5310         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5311         assertNull(calibrator.getEstimatedBiasFx());
5312         assertNull(calibrator.getEstimatedBiasFy());
5313         assertNull(calibrator.getEstimatedBiasFz());
5314         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5315         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5316         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5317         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5318         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5319         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5320         assertNull(calibrator.getEstimatedMa());
5321         assertNull(calibrator.getEstimatedSx());
5322         assertNull(calibrator.getEstimatedSy());
5323         assertNull(calibrator.getEstimatedSz());
5324         assertNull(calibrator.getEstimatedMxy());
5325         assertNull(calibrator.getEstimatedMxz());
5326         assertNull(calibrator.getEstimatedMyx());
5327         assertNull(calibrator.getEstimatedMyz());
5328         assertNull(calibrator.getEstimatedMzx());
5329         assertNull(calibrator.getEstimatedMzy());
5330         assertNull(calibrator.getEstimatedCovariance());
5331         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5332         assertNull(calibrator.getGroundTruthGravityNorm());
5333         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5334         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5335     }
5336 
5337     @Test
5338     public void testConstructor49() throws WrongSizeException {
5339 
5340         final Collection<StandardDeviationBodyKinematics> measurements =
5341                 Collections.emptyList();
5342 
5343         final Matrix ba = generateBa();
5344         final double biasX = ba.getElementAtIndex(0);
5345         final double biasY = ba.getElementAtIndex(1);
5346         final double biasZ = ba.getElementAtIndex(2);
5347 
5348         final Matrix ma = generateMaCommonAxis();
5349         final double sx = ma.getElementAt(0, 0);
5350         final double sy = ma.getElementAt(1, 1);
5351         final double sz = ma.getElementAt(2, 2);
5352         final double mxy = ma.getElementAt(0, 1);
5353         final double mxz = ma.getElementAt(0, 2);
5354         final double myx = ma.getElementAt(1, 0);
5355         final double myz = ma.getElementAt(1, 2);
5356         final double mzx = ma.getElementAt(2, 0);
5357         final double mzy = ma.getElementAt(2, 1);
5358 
5359         final Acceleration bx = new Acceleration(biasX,
5360                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5361         final Acceleration by = new Acceleration(biasY,
5362                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5363         final Acceleration bz = new Acceleration(biasZ,
5364                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5365 
5366         final KnownPositionAccelerometerCalibrator calibrator =
5367                 new KnownPositionAccelerometerCalibrator(measurements, bx, by, bz,
5368                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5369 
5370         // check default values
5371         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5372         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5373         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5374         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5375         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5376         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5377         final Acceleration bx2 = new Acceleration(0.0,
5378                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5379         calibrator.getInitialBiasXAsAcceleration(bx2);
5380         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5381         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5382         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5383         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5384         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5385         final Acceleration by2 = new Acceleration(0.0,
5386                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5387         calibrator.getInitialBiasYAsAcceleration(by2);
5388         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5389         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5390         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5391         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5392         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5393         final Acceleration bz2 = new Acceleration(0.0,
5394                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5395         calibrator.getInitialBiasZAsAcceleration(bz2);
5396         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5397         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5398         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5399         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5400         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5401         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5402         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5403         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5404         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5405         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5406         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5407         final double[] bias1 = calibrator.getInitialBias();
5408         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5409         final double[] bias2 = new double[3];
5410         calibrator.getInitialBias(bias2);
5411         assertArrayEquals(bias1, bias2, 0.0);
5412         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5413         assertEquals(b1, ba);
5414         final Matrix b2 = new Matrix(3, 1);
5415         calibrator.getInitialBiasAsMatrix(b2);
5416         assertEquals(b1, b2);
5417         final Matrix ma1 = new Matrix(3, 3);
5418         ma1.setSubmatrix(0, 0,
5419                 2, 2,
5420                 new double[]{sx, myx, mzx,
5421                         mxy, sy, mzy,
5422                         mxz, myz, sz});
5423         assertEquals(calibrator.getInitialMa(), ma1);
5424         final Matrix ma2 = new Matrix(3, 3);
5425         calibrator.getInitialMa(ma2);
5426         assertEquals(ma1, ma2);
5427         assertNull(calibrator.getEcefPosition());
5428         assertNull(calibrator.getNedPosition());
5429         assertFalse(calibrator.getNedPosition(null));
5430         assertSame(calibrator.getMeasurements(), measurements);
5431         assertFalse(calibrator.isCommonAxisUsed());
5432         assertNull(calibrator.getListener());
5433         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5434         assertFalse(calibrator.isReady());
5435         assertFalse(calibrator.isRunning());
5436         assertNull(calibrator.getEstimatedBiases());
5437         assertFalse(calibrator.getEstimatedBiases(null));
5438         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5439         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5440         assertNull(calibrator.getEstimatedBiasFx());
5441         assertNull(calibrator.getEstimatedBiasFy());
5442         assertNull(calibrator.getEstimatedBiasFz());
5443         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5444         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5445         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5446         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5447         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5448         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5449         assertNull(calibrator.getEstimatedMa());
5450         assertNull(calibrator.getEstimatedSx());
5451         assertNull(calibrator.getEstimatedSy());
5452         assertNull(calibrator.getEstimatedSz());
5453         assertNull(calibrator.getEstimatedMxy());
5454         assertNull(calibrator.getEstimatedMxz());
5455         assertNull(calibrator.getEstimatedMyx());
5456         assertNull(calibrator.getEstimatedMyz());
5457         assertNull(calibrator.getEstimatedMzx());
5458         assertNull(calibrator.getEstimatedMzy());
5459         assertNull(calibrator.getEstimatedCovariance());
5460         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5461         assertNull(calibrator.getGroundTruthGravityNorm());
5462         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5463         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5464     }
5465 
5466     @Test
5467     public void testConstructor50() throws WrongSizeException {
5468 
5469         final Collection<StandardDeviationBodyKinematics> measurements =
5470                 Collections.emptyList();
5471 
5472         final Matrix ba = generateBa();
5473         final double biasX = ba.getElementAtIndex(0);
5474         final double biasY = ba.getElementAtIndex(1);
5475         final double biasZ = ba.getElementAtIndex(2);
5476 
5477         final Matrix ma = generateMaCommonAxis();
5478         final double sx = ma.getElementAt(0, 0);
5479         final double sy = ma.getElementAt(1, 1);
5480         final double sz = ma.getElementAt(2, 2);
5481         final double mxy = ma.getElementAt(0, 1);
5482         final double mxz = ma.getElementAt(0, 2);
5483         final double myx = ma.getElementAt(1, 0);
5484         final double myz = ma.getElementAt(1, 2);
5485         final double mzx = ma.getElementAt(2, 0);
5486         final double mzy = ma.getElementAt(2, 1);
5487 
5488         final Acceleration bx = new Acceleration(biasX,
5489                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5490         final Acceleration by = new Acceleration(biasY,
5491                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5492         final Acceleration bz = new Acceleration(biasZ,
5493                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5494 
5495         final KnownPositionAccelerometerCalibrator calibrator =
5496                 new KnownPositionAccelerometerCalibrator(measurements, bx, by, bz,
5497                         sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5498 
5499         // check default values
5500         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5501         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5502         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5503         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5504         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5505         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5506         final Acceleration bx2 = new Acceleration(0.0,
5507                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5508         calibrator.getInitialBiasXAsAcceleration(bx2);
5509         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5510         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5511         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5512         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5513         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5514         final Acceleration by2 = new Acceleration(0.0,
5515                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5516         calibrator.getInitialBiasYAsAcceleration(by2);
5517         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5518         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5519         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5520         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5521         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5522         final Acceleration bz2 = new Acceleration(0.0,
5523                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5524         calibrator.getInitialBiasZAsAcceleration(bz2);
5525         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5526         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5527         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5528         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5529         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5530         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5531         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5532         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5533         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5534         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5535         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5536         final double[] bias1 = calibrator.getInitialBias();
5537         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5538         final double[] bias2 = new double[3];
5539         calibrator.getInitialBias(bias2);
5540         assertArrayEquals(bias1, bias2, 0.0);
5541         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5542         assertEquals(b1, ba);
5543         final Matrix b2 = new Matrix(3, 1);
5544         calibrator.getInitialBiasAsMatrix(b2);
5545         assertEquals(b1, b2);
5546         final Matrix ma1 = new Matrix(3, 3);
5547         ma1.setSubmatrix(0, 0,
5548                 2, 2,
5549                 new double[]{sx, myx, mzx,
5550                         mxy, sy, mzy,
5551                         mxz, myz, sz});
5552         assertEquals(calibrator.getInitialMa(), ma1);
5553         final Matrix ma2 = new Matrix(3, 3);
5554         calibrator.getInitialMa(ma2);
5555         assertEquals(ma1, ma2);
5556         assertNull(calibrator.getEcefPosition());
5557         assertNull(calibrator.getNedPosition());
5558         assertFalse(calibrator.getNedPosition(null));
5559         assertSame(calibrator.getMeasurements(), measurements);
5560         assertFalse(calibrator.isCommonAxisUsed());
5561         assertSame(calibrator.getListener(), this);
5562         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5563         assertFalse(calibrator.isReady());
5564         assertFalse(calibrator.isRunning());
5565         assertNull(calibrator.getEstimatedBiases());
5566         assertFalse(calibrator.getEstimatedBiases(null));
5567         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5568         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5569         assertNull(calibrator.getEstimatedBiasFx());
5570         assertNull(calibrator.getEstimatedBiasFy());
5571         assertNull(calibrator.getEstimatedBiasFz());
5572         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5573         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5574         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5575         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5576         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5577         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5578         assertNull(calibrator.getEstimatedMa());
5579         assertNull(calibrator.getEstimatedSx());
5580         assertNull(calibrator.getEstimatedSy());
5581         assertNull(calibrator.getEstimatedSz());
5582         assertNull(calibrator.getEstimatedMxy());
5583         assertNull(calibrator.getEstimatedMxz());
5584         assertNull(calibrator.getEstimatedMyx());
5585         assertNull(calibrator.getEstimatedMyz());
5586         assertNull(calibrator.getEstimatedMzx());
5587         assertNull(calibrator.getEstimatedMzy());
5588         assertNull(calibrator.getEstimatedCovariance());
5589         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5590         assertNull(calibrator.getGroundTruthGravityNorm());
5591         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5592         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5593     }
5594 
5595     @Test
5596     public void testConstructor51() throws WrongSizeException {
5597         final Matrix ba = generateBa();
5598         final double biasX = ba.getElementAtIndex(0);
5599         final double biasY = ba.getElementAtIndex(1);
5600         final double biasZ = ba.getElementAtIndex(2);
5601 
5602         final Matrix ma = generateMaCommonAxis();
5603         final double sx = ma.getElementAt(0, 0);
5604         final double sy = ma.getElementAt(1, 1);
5605         final double sz = ma.getElementAt(2, 2);
5606         final double mxy = ma.getElementAt(0, 1);
5607         final double mxz = ma.getElementAt(0, 2);
5608         final double myx = ma.getElementAt(1, 0);
5609         final double myz = ma.getElementAt(1, 2);
5610         final double mzx = ma.getElementAt(2, 0);
5611         final double mzy = ma.getElementAt(2, 1);
5612 
5613         final Acceleration bx = new Acceleration(biasX,
5614                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5615         final Acceleration by = new Acceleration(biasY,
5616                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5617         final Acceleration bz = new Acceleration(biasZ,
5618                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5619 
5620         final KnownPositionAccelerometerCalibrator calibrator =
5621                 new KnownPositionAccelerometerCalibrator(true,
5622                         bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5623 
5624         // check default values
5625         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5626         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5627         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5628         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5629         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5630         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5631         final Acceleration bx2 = new Acceleration(0.0,
5632                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5633         calibrator.getInitialBiasXAsAcceleration(bx2);
5634         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5635         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5636         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5637         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5638         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5639         final Acceleration by2 = new Acceleration(0.0,
5640                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5641         calibrator.getInitialBiasYAsAcceleration(by2);
5642         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5643         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5644         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5645         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5646         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5647         final Acceleration bz2 = new Acceleration(0.0,
5648                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5649         calibrator.getInitialBiasZAsAcceleration(bz2);
5650         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5651         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5652         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5653         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5654         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5655         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5656         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5657         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5658         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5659         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5660         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5661         final double[] bias1 = calibrator.getInitialBias();
5662         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5663         final double[] bias2 = new double[3];
5664         calibrator.getInitialBias(bias2);
5665         assertArrayEquals(bias1, bias2, 0.0);
5666         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5667         assertEquals(b1, ba);
5668         final Matrix b2 = new Matrix(3, 1);
5669         calibrator.getInitialBiasAsMatrix(b2);
5670         assertEquals(b1, b2);
5671         final Matrix ma1 = new Matrix(3, 3);
5672         ma1.setSubmatrix(0, 0,
5673                 2, 2,
5674                 new double[]{sx, myx, mzx,
5675                         mxy, sy, mzy,
5676                         mxz, myz, sz});
5677         assertEquals(calibrator.getInitialMa(), ma1);
5678         final Matrix ma2 = new Matrix(3, 3);
5679         calibrator.getInitialMa(ma2);
5680         assertEquals(ma1, ma2);
5681         assertNull(calibrator.getEcefPosition());
5682         assertNull(calibrator.getNedPosition());
5683         assertFalse(calibrator.getNedPosition(null));
5684         assertNull(calibrator.getMeasurements());
5685         assertTrue(calibrator.isCommonAxisUsed());
5686         assertNull(calibrator.getListener());
5687         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5688         assertFalse(calibrator.isReady());
5689         assertFalse(calibrator.isRunning());
5690         assertNull(calibrator.getEstimatedBiases());
5691         assertFalse(calibrator.getEstimatedBiases(null));
5692         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5693         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5694         assertNull(calibrator.getEstimatedBiasFx());
5695         assertNull(calibrator.getEstimatedBiasFy());
5696         assertNull(calibrator.getEstimatedBiasFz());
5697         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5698         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5699         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5700         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5701         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5702         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5703         assertNull(calibrator.getEstimatedMa());
5704         assertNull(calibrator.getEstimatedSx());
5705         assertNull(calibrator.getEstimatedSy());
5706         assertNull(calibrator.getEstimatedSz());
5707         assertNull(calibrator.getEstimatedMxy());
5708         assertNull(calibrator.getEstimatedMxz());
5709         assertNull(calibrator.getEstimatedMyx());
5710         assertNull(calibrator.getEstimatedMyz());
5711         assertNull(calibrator.getEstimatedMzx());
5712         assertNull(calibrator.getEstimatedMzy());
5713         assertNull(calibrator.getEstimatedCovariance());
5714         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5715         assertNull(calibrator.getGroundTruthGravityNorm());
5716         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5717         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5718     }
5719 
5720     @Test
5721     public void testConstructor52() throws WrongSizeException {
5722         final Matrix ba = generateBa();
5723         final double biasX = ba.getElementAtIndex(0);
5724         final double biasY = ba.getElementAtIndex(1);
5725         final double biasZ = ba.getElementAtIndex(2);
5726 
5727         final Matrix ma = generateMaCommonAxis();
5728         final double sx = ma.getElementAt(0, 0);
5729         final double sy = ma.getElementAt(1, 1);
5730         final double sz = ma.getElementAt(2, 2);
5731         final double mxy = ma.getElementAt(0, 1);
5732         final double mxz = ma.getElementAt(0, 2);
5733         final double myx = ma.getElementAt(1, 0);
5734         final double myz = ma.getElementAt(1, 2);
5735         final double mzx = ma.getElementAt(2, 0);
5736         final double mzy = ma.getElementAt(2, 1);
5737 
5738         final Acceleration bx = new Acceleration(biasX,
5739                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5740         final Acceleration by = new Acceleration(biasY,
5741                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5742         final Acceleration bz = new Acceleration(biasZ,
5743                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5744 
5745         final KnownPositionAccelerometerCalibrator calibrator =
5746                 new KnownPositionAccelerometerCalibrator(true,
5747                         bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
5748                         this);
5749 
5750         // check default values
5751         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5752         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5753         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5754         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5755         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5756         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5757         final Acceleration bx2 = new Acceleration(0.0,
5758                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5759         calibrator.getInitialBiasXAsAcceleration(bx2);
5760         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5761         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5762         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5763         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5764         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5765         final Acceleration by2 = new Acceleration(0.0,
5766                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5767         calibrator.getInitialBiasYAsAcceleration(by2);
5768         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5769         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5770         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5771         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5772         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5773         final Acceleration bz2 = new Acceleration(0.0,
5774                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5775         calibrator.getInitialBiasZAsAcceleration(bz2);
5776         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5777         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5778         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5779         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5780         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5781         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5782         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5783         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5784         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5785         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5786         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5787         final double[] bias1 = calibrator.getInitialBias();
5788         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5789         final double[] bias2 = new double[3];
5790         calibrator.getInitialBias(bias2);
5791         assertArrayEquals(bias1, bias2, 0.0);
5792         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5793         assertEquals(b1, ba);
5794         final Matrix b2 = new Matrix(3, 1);
5795         calibrator.getInitialBiasAsMatrix(b2);
5796         assertEquals(b1, b2);
5797         final Matrix ma1 = new Matrix(3, 3);
5798         ma1.setSubmatrix(0, 0,
5799                 2, 2,
5800                 new double[]{sx, myx, mzx,
5801                         mxy, sy, mzy,
5802                         mxz, myz, sz});
5803         assertEquals(calibrator.getInitialMa(), ma1);
5804         final Matrix ma2 = new Matrix(3, 3);
5805         calibrator.getInitialMa(ma2);
5806         assertEquals(ma1, ma2);
5807         assertNull(calibrator.getEcefPosition());
5808         assertNull(calibrator.getNedPosition());
5809         assertFalse(calibrator.getNedPosition(null));
5810         assertNull(calibrator.getMeasurements());
5811         assertTrue(calibrator.isCommonAxisUsed());
5812         assertSame(calibrator.getListener(), this);
5813         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5814         assertFalse(calibrator.isReady());
5815         assertFalse(calibrator.isRunning());
5816         assertNull(calibrator.getEstimatedBiases());
5817         assertFalse(calibrator.getEstimatedBiases(null));
5818         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5819         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5820         assertNull(calibrator.getEstimatedBiasFx());
5821         assertNull(calibrator.getEstimatedBiasFy());
5822         assertNull(calibrator.getEstimatedBiasFz());
5823         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5824         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5825         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5826         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5827         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5828         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5829         assertNull(calibrator.getEstimatedMa());
5830         assertNull(calibrator.getEstimatedSx());
5831         assertNull(calibrator.getEstimatedSy());
5832         assertNull(calibrator.getEstimatedSz());
5833         assertNull(calibrator.getEstimatedMxy());
5834         assertNull(calibrator.getEstimatedMxz());
5835         assertNull(calibrator.getEstimatedMyx());
5836         assertNull(calibrator.getEstimatedMyz());
5837         assertNull(calibrator.getEstimatedMzx());
5838         assertNull(calibrator.getEstimatedMzy());
5839         assertNull(calibrator.getEstimatedCovariance());
5840         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5841         assertNull(calibrator.getGroundTruthGravityNorm());
5842         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5843         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5844     }
5845 
5846     @Test
5847     public void testConstructor53() throws WrongSizeException {
5848         final Collection<StandardDeviationBodyKinematics> measurements =
5849                 Collections.emptyList();
5850 
5851         final Matrix ba = generateBa();
5852         final double biasX = ba.getElementAtIndex(0);
5853         final double biasY = ba.getElementAtIndex(1);
5854         final double biasZ = ba.getElementAtIndex(2);
5855 
5856         final Matrix ma = generateMaCommonAxis();
5857         final double sx = ma.getElementAt(0, 0);
5858         final double sy = ma.getElementAt(1, 1);
5859         final double sz = ma.getElementAt(2, 2);
5860         final double mxy = ma.getElementAt(0, 1);
5861         final double mxz = ma.getElementAt(0, 2);
5862         final double myx = ma.getElementAt(1, 0);
5863         final double myz = ma.getElementAt(1, 2);
5864         final double mzx = ma.getElementAt(2, 0);
5865         final double mzy = ma.getElementAt(2, 1);
5866 
5867         final Acceleration bx = new Acceleration(biasX,
5868                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5869         final Acceleration by = new Acceleration(biasY,
5870                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5871         final Acceleration bz = new Acceleration(biasZ,
5872                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5873 
5874         final KnownPositionAccelerometerCalibrator calibrator =
5875                 new KnownPositionAccelerometerCalibrator(measurements,
5876                         true, bx, by, bz, sx, sy, sz,
5877                         mxy, mxz, myx, myz, mzx, mzy);
5878 
5879         // check default values
5880         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5881         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5882         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5883         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5884         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5885         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5886         final Acceleration bx2 = new Acceleration(0.0,
5887                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5888         calibrator.getInitialBiasXAsAcceleration(bx2);
5889         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5890         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5891         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5892         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5893         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5894         final Acceleration by2 = new Acceleration(0.0,
5895                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5896         calibrator.getInitialBiasYAsAcceleration(by2);
5897         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5898         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5899         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5900         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5901         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5902         final Acceleration bz2 = new Acceleration(0.0,
5903                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5904         calibrator.getInitialBiasZAsAcceleration(bz2);
5905         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5906         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5907         assertEquals(calibrator.getInitialSx(), sx, 0.0);
5908         assertEquals(calibrator.getInitialSy(), sy, 0.0);
5909         assertEquals(calibrator.getInitialSz(), sz, 0.0);
5910         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5911         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5912         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5913         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5914         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5915         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5916         final double[] bias1 = calibrator.getInitialBias();
5917         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5918         final double[] bias2 = new double[3];
5919         calibrator.getInitialBias(bias2);
5920         assertArrayEquals(bias1, bias2, 0.0);
5921         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5922         assertEquals(b1, ba);
5923         final Matrix b2 = new Matrix(3, 1);
5924         calibrator.getInitialBiasAsMatrix(b2);
5925         assertEquals(b1, b2);
5926         final Matrix ma1 = new Matrix(3, 3);
5927         ma1.setSubmatrix(0, 0,
5928                 2, 2,
5929                 new double[]{sx, myx, mzx,
5930                         mxy, sy, mzy,
5931                         mxz, myz, sz});
5932         assertEquals(calibrator.getInitialMa(), ma1);
5933         final Matrix ma2 = new Matrix(3, 3);
5934         calibrator.getInitialMa(ma2);
5935         assertEquals(ma1, ma2);
5936         assertNull(calibrator.getEcefPosition());
5937         assertNull(calibrator.getNedPosition());
5938         assertFalse(calibrator.getNedPosition(null));
5939         assertSame(calibrator.getMeasurements(), measurements);
5940         assertTrue(calibrator.isCommonAxisUsed());
5941         assertNull(calibrator.getListener());
5942         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5943         assertFalse(calibrator.isReady());
5944         assertFalse(calibrator.isRunning());
5945         assertNull(calibrator.getEstimatedBiases());
5946         assertFalse(calibrator.getEstimatedBiases(null));
5947         assertNull(calibrator.getEstimatedBiasesAsMatrix());
5948         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5949         assertNull(calibrator.getEstimatedBiasFx());
5950         assertNull(calibrator.getEstimatedBiasFy());
5951         assertNull(calibrator.getEstimatedBiasFz());
5952         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5953         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5954         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5955         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5956         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5957         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5958         assertNull(calibrator.getEstimatedMa());
5959         assertNull(calibrator.getEstimatedSx());
5960         assertNull(calibrator.getEstimatedSy());
5961         assertNull(calibrator.getEstimatedSz());
5962         assertNull(calibrator.getEstimatedMxy());
5963         assertNull(calibrator.getEstimatedMxz());
5964         assertNull(calibrator.getEstimatedMyx());
5965         assertNull(calibrator.getEstimatedMyz());
5966         assertNull(calibrator.getEstimatedMzx());
5967         assertNull(calibrator.getEstimatedMzy());
5968         assertNull(calibrator.getEstimatedCovariance());
5969         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5970         assertNull(calibrator.getGroundTruthGravityNorm());
5971         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5972         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5973     }
5974 
5975     @Test
5976     public void testConstructor54() throws WrongSizeException {
5977         final Collection<StandardDeviationBodyKinematics> measurements =
5978                 Collections.emptyList();
5979 
5980         final Matrix ba = generateBa();
5981         final double biasX = ba.getElementAtIndex(0);
5982         final double biasY = ba.getElementAtIndex(1);
5983         final double biasZ = ba.getElementAtIndex(2);
5984 
5985         final Matrix ma = generateMaCommonAxis();
5986         final double sx = ma.getElementAt(0, 0);
5987         final double sy = ma.getElementAt(1, 1);
5988         final double sz = ma.getElementAt(2, 2);
5989         final double mxy = ma.getElementAt(0, 1);
5990         final double mxz = ma.getElementAt(0, 2);
5991         final double myx = ma.getElementAt(1, 0);
5992         final double myz = ma.getElementAt(1, 2);
5993         final double mzx = ma.getElementAt(2, 0);
5994         final double mzy = ma.getElementAt(2, 1);
5995 
5996         final Acceleration bx = new Acceleration(biasX,
5997                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5998         final Acceleration by = new Acceleration(biasY,
5999                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
6000         final Acceleration bz = new Acceleration(biasZ,
6001                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
6002 
6003         final KnownPositionAccelerometerCalibrator calibrator =
6004                 new KnownPositionAccelerometerCalibrator(measurements,
6005                         true, bx, by, bz, sx, sy, sz,
6006                         mxy, mxz, myx, myz, mzx, mzy, this);
6007 
6008         // check default values
6009         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6010         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6011         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6012         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6013         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6014         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6015         final Acceleration bx2 = new Acceleration(0.0,
6016                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6017         calibrator.getInitialBiasXAsAcceleration(bx2);
6018         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6019         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6020         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6021         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6022         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6023         final Acceleration by2 = new Acceleration(0.0,
6024                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6025         calibrator.getInitialBiasYAsAcceleration(by2);
6026         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6027         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6028         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6029         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6030         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6031         final Acceleration bz2 = new Acceleration(0.0,
6032                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6033         calibrator.getInitialBiasZAsAcceleration(bz2);
6034         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6035         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6036         assertEquals(calibrator.getInitialSx(), sx, 0.0);
6037         assertEquals(calibrator.getInitialSy(), sy, 0.0);
6038         assertEquals(calibrator.getInitialSz(), sz, 0.0);
6039         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
6040         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
6041         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
6042         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
6043         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
6044         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
6045         final double[] bias1 = calibrator.getInitialBias();
6046         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6047         final double[] bias2 = new double[3];
6048         calibrator.getInitialBias(bias2);
6049         assertArrayEquals(bias1, bias2, 0.0);
6050         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6051         assertEquals(b1, ba);
6052         final Matrix b2 = new Matrix(3, 1);
6053         calibrator.getInitialBiasAsMatrix(b2);
6054         assertEquals(b1, b2);
6055         final Matrix ma1 = new Matrix(3, 3);
6056         ma1.setSubmatrix(0, 0,
6057                 2, 2,
6058                 new double[]{sx, myx, mzx,
6059                         mxy, sy, mzy,
6060                         mxz, myz, sz});
6061         assertEquals(calibrator.getInitialMa(), ma1);
6062         final Matrix ma2 = new Matrix(3, 3);
6063         calibrator.getInitialMa(ma2);
6064         assertEquals(ma1, ma2);
6065         assertNull(calibrator.getEcefPosition());
6066         assertNull(calibrator.getNedPosition());
6067         assertFalse(calibrator.getNedPosition(null));
6068         assertSame(calibrator.getMeasurements(), measurements);
6069         assertTrue(calibrator.isCommonAxisUsed());
6070         assertSame(calibrator.getListener(), this);
6071         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6072         assertFalse(calibrator.isReady());
6073         assertFalse(calibrator.isRunning());
6074         assertNull(calibrator.getEstimatedBiases());
6075         assertFalse(calibrator.getEstimatedBiases(null));
6076         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6077         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6078         assertNull(calibrator.getEstimatedBiasFx());
6079         assertNull(calibrator.getEstimatedBiasFy());
6080         assertNull(calibrator.getEstimatedBiasFz());
6081         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6082         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6083         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6084         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6085         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6086         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6087         assertNull(calibrator.getEstimatedMa());
6088         assertNull(calibrator.getEstimatedSx());
6089         assertNull(calibrator.getEstimatedSy());
6090         assertNull(calibrator.getEstimatedSz());
6091         assertNull(calibrator.getEstimatedMxy());
6092         assertNull(calibrator.getEstimatedMxz());
6093         assertNull(calibrator.getEstimatedMyx());
6094         assertNull(calibrator.getEstimatedMyz());
6095         assertNull(calibrator.getEstimatedMzx());
6096         assertNull(calibrator.getEstimatedMzy());
6097         assertNull(calibrator.getEstimatedCovariance());
6098         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6099         assertNull(calibrator.getGroundTruthGravityNorm());
6100         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6101         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6102     }
6103 
6104     @Test
6105     public void testConstructor55() throws WrongSizeException {
6106         final Matrix ba = generateBa();
6107         final double[] bias = ba.getBuffer();
6108         final double biasX = ba.getElementAtIndex(0);
6109         final double biasY = ba.getElementAtIndex(1);
6110         final double biasZ = ba.getElementAtIndex(2);
6111 
6112         KnownPositionAccelerometerCalibrator calibrator =
6113                 new KnownPositionAccelerometerCalibrator(bias);
6114 
6115         // check default values
6116         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6117         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6118         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6119         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6120         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6121         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6122         final Acceleration bx2 = new Acceleration(0.0,
6123                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6124         calibrator.getInitialBiasXAsAcceleration(bx2);
6125         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6126         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6127         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6128         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6129         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6130         final Acceleration by2 = new Acceleration(0.0,
6131                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6132         calibrator.getInitialBiasYAsAcceleration(by2);
6133         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6134         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6135         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6136         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6137         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6138         final Acceleration bz2 = new Acceleration(0.0,
6139                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6140         calibrator.getInitialBiasZAsAcceleration(bz2);
6141         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6142         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6143         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6144         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6145         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6146         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6147         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6148         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6149         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6150         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6151         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6152         final double[] bias1 = calibrator.getInitialBias();
6153         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6154         final double[] bias2 = new double[3];
6155         calibrator.getInitialBias(bias2);
6156         assertArrayEquals(bias1, bias2, 0.0);
6157         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6158         assertEquals(b1, ba);
6159         final Matrix b2 = new Matrix(3, 1);
6160         calibrator.getInitialBiasAsMatrix(b2);
6161         assertEquals(b1, b2);
6162         final Matrix ma1 = calibrator.getInitialMa();
6163         assertEquals(ma1, new Matrix(3, 3));
6164         final Matrix ma2 = new Matrix(3, 3);
6165         calibrator.getInitialMa(ma2);
6166         assertEquals(ma1, ma2);
6167         assertNull(calibrator.getEcefPosition());
6168         assertNull(calibrator.getNedPosition());
6169         assertFalse(calibrator.getNedPosition(null));
6170         assertNull(calibrator.getMeasurements());
6171         assertFalse(calibrator.isCommonAxisUsed());
6172         assertNull(calibrator.getListener());
6173         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6174         assertFalse(calibrator.isReady());
6175         assertFalse(calibrator.isRunning());
6176         assertNull(calibrator.getEstimatedBiases());
6177         assertFalse(calibrator.getEstimatedBiases(null));
6178         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6179         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6180         assertNull(calibrator.getEstimatedBiasFx());
6181         assertNull(calibrator.getEstimatedBiasFy());
6182         assertNull(calibrator.getEstimatedBiasFz());
6183         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6184         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6185         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6186         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6187         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6188         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6189         assertNull(calibrator.getEstimatedMa());
6190         assertNull(calibrator.getEstimatedSx());
6191         assertNull(calibrator.getEstimatedSy());
6192         assertNull(calibrator.getEstimatedSz());
6193         assertNull(calibrator.getEstimatedMxy());
6194         assertNull(calibrator.getEstimatedMxz());
6195         assertNull(calibrator.getEstimatedMyx());
6196         assertNull(calibrator.getEstimatedMyz());
6197         assertNull(calibrator.getEstimatedMzx());
6198         assertNull(calibrator.getEstimatedMzy());
6199         assertNull(calibrator.getEstimatedCovariance());
6200         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6201         assertNull(calibrator.getGroundTruthGravityNorm());
6202         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6203         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6204 
6205         // Force IllegalArgumentException
6206         calibrator = null;
6207         try {
6208             calibrator = new KnownPositionAccelerometerCalibrator(new double[1]);
6209             fail("IllegalArgumentException expected but not thrown");
6210         } catch (final IllegalArgumentException ignore) {
6211         }
6212         assertNull(calibrator);
6213     }
6214 
6215     @Test
6216     public void testConstructor56() throws WrongSizeException {
6217         final Matrix ba = generateBa();
6218         final double[] bias = ba.getBuffer();
6219         final double biasX = ba.getElementAtIndex(0);
6220         final double biasY = ba.getElementAtIndex(1);
6221         final double biasZ = ba.getElementAtIndex(2);
6222 
6223         KnownPositionAccelerometerCalibrator calibrator =
6224                 new KnownPositionAccelerometerCalibrator(bias, this);
6225 
6226         // check default values
6227         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6228         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6229         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6230         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6231         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6232         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6233         final Acceleration bx2 = new Acceleration(0.0,
6234                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6235         calibrator.getInitialBiasXAsAcceleration(bx2);
6236         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6237         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6238         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6239         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6240         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6241         final Acceleration by2 = new Acceleration(0.0,
6242                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6243         calibrator.getInitialBiasYAsAcceleration(by2);
6244         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6245         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6246         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6247         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6248         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6249         final Acceleration bz2 = new Acceleration(0.0,
6250                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6251         calibrator.getInitialBiasZAsAcceleration(bz2);
6252         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6253         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6254         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6255         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6256         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6257         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6258         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6259         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6260         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6261         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6262         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6263         final double[] bias1 = calibrator.getInitialBias();
6264         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6265         final double[] bias2 = new double[3];
6266         calibrator.getInitialBias(bias2);
6267         assertArrayEquals(bias1, bias2, 0.0);
6268         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6269         assertEquals(b1, ba);
6270         final Matrix b2 = new Matrix(3, 1);
6271         calibrator.getInitialBiasAsMatrix(b2);
6272         assertEquals(b1, b2);
6273         final Matrix ma1 = calibrator.getInitialMa();
6274         assertEquals(ma1, new Matrix(3, 3));
6275         final Matrix ma2 = new Matrix(3, 3);
6276         calibrator.getInitialMa(ma2);
6277         assertEquals(ma1, ma2);
6278         assertNull(calibrator.getEcefPosition());
6279         assertNull(calibrator.getNedPosition());
6280         assertFalse(calibrator.getNedPosition(null));
6281         assertNull(calibrator.getMeasurements());
6282         assertFalse(calibrator.isCommonAxisUsed());
6283         assertSame(calibrator.getListener(), this);
6284         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6285         assertFalse(calibrator.isReady());
6286         assertFalse(calibrator.isRunning());
6287         assertNull(calibrator.getEstimatedBiases());
6288         assertFalse(calibrator.getEstimatedBiases(null));
6289         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6290         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6291         assertNull(calibrator.getEstimatedBiasFx());
6292         assertNull(calibrator.getEstimatedBiasFy());
6293         assertNull(calibrator.getEstimatedBiasFz());
6294         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6295         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6296         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6297         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6298         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6299         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6300         assertNull(calibrator.getEstimatedMa());
6301         assertNull(calibrator.getEstimatedSx());
6302         assertNull(calibrator.getEstimatedSy());
6303         assertNull(calibrator.getEstimatedSz());
6304         assertNull(calibrator.getEstimatedMxy());
6305         assertNull(calibrator.getEstimatedMxz());
6306         assertNull(calibrator.getEstimatedMyx());
6307         assertNull(calibrator.getEstimatedMyz());
6308         assertNull(calibrator.getEstimatedMzx());
6309         assertNull(calibrator.getEstimatedMzy());
6310         assertNull(calibrator.getEstimatedCovariance());
6311         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6312         assertNull(calibrator.getGroundTruthGravityNorm());
6313         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6314         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6315 
6316         // Force IllegalArgumentException
6317         calibrator = null;
6318         try {
6319             calibrator = new KnownPositionAccelerometerCalibrator(new double[1],
6320                     this);
6321             fail("IllegalArgumentException expected but not thrown");
6322         } catch (final IllegalArgumentException ignore) {
6323         }
6324         assertNull(calibrator);
6325     }
6326 
6327     @Test
6328     public void testConstructor57() throws WrongSizeException {
6329         final Collection<StandardDeviationBodyKinematics> measurements =
6330                 Collections.emptyList();
6331 
6332         final Matrix ba = generateBa();
6333         final double[] bias = ba.getBuffer();
6334         final double biasX = ba.getElementAtIndex(0);
6335         final double biasY = ba.getElementAtIndex(1);
6336         final double biasZ = ba.getElementAtIndex(2);
6337 
6338         KnownPositionAccelerometerCalibrator calibrator =
6339                 new KnownPositionAccelerometerCalibrator(measurements, bias);
6340 
6341         // check default values
6342         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6343         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6344         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6345         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6346         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6347         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6348         final Acceleration bx2 = new Acceleration(0.0,
6349                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6350         calibrator.getInitialBiasXAsAcceleration(bx2);
6351         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6352         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6353         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6354         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6355         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6356         final Acceleration by2 = new Acceleration(0.0,
6357                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6358         calibrator.getInitialBiasYAsAcceleration(by2);
6359         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6360         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6361         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6362         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6363         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6364         final Acceleration bz2 = new Acceleration(0.0,
6365                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6366         calibrator.getInitialBiasZAsAcceleration(bz2);
6367         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6368         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6369         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6370         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6371         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6372         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6373         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6374         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6375         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6376         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6377         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6378         final double[] bias1 = calibrator.getInitialBias();
6379         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6380         final double[] bias2 = new double[3];
6381         calibrator.getInitialBias(bias2);
6382         assertArrayEquals(bias1, bias2, 0.0);
6383         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6384         assertEquals(b1, ba);
6385         final Matrix b2 = new Matrix(3, 1);
6386         calibrator.getInitialBiasAsMatrix(b2);
6387         assertEquals(b1, b2);
6388         final Matrix ma1 = calibrator.getInitialMa();
6389         assertEquals(ma1, new Matrix(3, 3));
6390         final Matrix ma2 = new Matrix(3, 3);
6391         calibrator.getInitialMa(ma2);
6392         assertEquals(ma1, ma2);
6393         assertNull(calibrator.getEcefPosition());
6394         assertNull(calibrator.getNedPosition());
6395         assertFalse(calibrator.getNedPosition(null));
6396         assertSame(calibrator.getMeasurements(), measurements);
6397         assertFalse(calibrator.isCommonAxisUsed());
6398         assertNull(calibrator.getListener());
6399         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6400         assertFalse(calibrator.isReady());
6401         assertFalse(calibrator.isRunning());
6402         assertNull(calibrator.getEstimatedBiases());
6403         assertFalse(calibrator.getEstimatedBiases(null));
6404         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6405         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6406         assertNull(calibrator.getEstimatedBiasFx());
6407         assertNull(calibrator.getEstimatedBiasFy());
6408         assertNull(calibrator.getEstimatedBiasFz());
6409         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6410         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6411         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6412         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6413         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6414         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6415         assertNull(calibrator.getEstimatedMa());
6416         assertNull(calibrator.getEstimatedSx());
6417         assertNull(calibrator.getEstimatedSy());
6418         assertNull(calibrator.getEstimatedSz());
6419         assertNull(calibrator.getEstimatedMxy());
6420         assertNull(calibrator.getEstimatedMxz());
6421         assertNull(calibrator.getEstimatedMyx());
6422         assertNull(calibrator.getEstimatedMyz());
6423         assertNull(calibrator.getEstimatedMzx());
6424         assertNull(calibrator.getEstimatedMzy());
6425         assertNull(calibrator.getEstimatedCovariance());
6426         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6427         assertNull(calibrator.getGroundTruthGravityNorm());
6428         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6429         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6430 
6431         // Force IllegalArgumentException
6432         calibrator = null;
6433         try {
6434             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6435                     new double[1]);
6436             fail("IllegalArgumentException expected but not thrown");
6437         } catch (final IllegalArgumentException ignore) {
6438         }
6439         assertNull(calibrator);
6440     }
6441 
6442     @Test
6443     public void testConstructor58() throws WrongSizeException {
6444         final Collection<StandardDeviationBodyKinematics> measurements =
6445                 Collections.emptyList();
6446 
6447         final Matrix ba = generateBa();
6448         final double[] bias = ba.getBuffer();
6449         final double biasX = ba.getElementAtIndex(0);
6450         final double biasY = ba.getElementAtIndex(1);
6451         final double biasZ = ba.getElementAtIndex(2);
6452 
6453         KnownPositionAccelerometerCalibrator calibrator =
6454                 new KnownPositionAccelerometerCalibrator(measurements, bias,
6455                         this);
6456 
6457         // check default values
6458         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6459         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6460         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6461         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6462         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6463         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6464         final Acceleration bx2 = new Acceleration(0.0,
6465                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6466         calibrator.getInitialBiasXAsAcceleration(bx2);
6467         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6468         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6469         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6470         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6471         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6472         final Acceleration by2 = new Acceleration(0.0,
6473                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6474         calibrator.getInitialBiasYAsAcceleration(by2);
6475         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6476         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6477         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6478         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6479         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6480         final Acceleration bz2 = new Acceleration(0.0,
6481                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6482         calibrator.getInitialBiasZAsAcceleration(bz2);
6483         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6484         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6485         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6486         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6487         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6488         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6489         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6490         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6491         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6492         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6493         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6494         final double[] bias1 = calibrator.getInitialBias();
6495         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6496         final double[] bias2 = new double[3];
6497         calibrator.getInitialBias(bias2);
6498         assertArrayEquals(bias1, bias2, 0.0);
6499         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6500         assertEquals(b1, ba);
6501         final Matrix b2 = new Matrix(3, 1);
6502         calibrator.getInitialBiasAsMatrix(b2);
6503         assertEquals(b1, b2);
6504         final Matrix ma1 = calibrator.getInitialMa();
6505         assertEquals(ma1, new Matrix(3, 3));
6506         final Matrix ma2 = new Matrix(3, 3);
6507         calibrator.getInitialMa(ma2);
6508         assertEquals(ma1, ma2);
6509         assertNull(calibrator.getEcefPosition());
6510         assertNull(calibrator.getNedPosition());
6511         assertFalse(calibrator.getNedPosition(null));
6512         assertSame(calibrator.getMeasurements(), measurements);
6513         assertFalse(calibrator.isCommonAxisUsed());
6514         assertSame(calibrator.getListener(), this);
6515         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6516         assertFalse(calibrator.isReady());
6517         assertFalse(calibrator.isRunning());
6518         assertNull(calibrator.getEstimatedBiases());
6519         assertFalse(calibrator.getEstimatedBiases(null));
6520         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6521         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6522         assertNull(calibrator.getEstimatedBiasFx());
6523         assertNull(calibrator.getEstimatedBiasFy());
6524         assertNull(calibrator.getEstimatedBiasFz());
6525         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6526         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6527         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6528         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6529         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6530         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6531         assertNull(calibrator.getEstimatedMa());
6532         assertNull(calibrator.getEstimatedSx());
6533         assertNull(calibrator.getEstimatedSy());
6534         assertNull(calibrator.getEstimatedSz());
6535         assertNull(calibrator.getEstimatedMxy());
6536         assertNull(calibrator.getEstimatedMxz());
6537         assertNull(calibrator.getEstimatedMyx());
6538         assertNull(calibrator.getEstimatedMyz());
6539         assertNull(calibrator.getEstimatedMzx());
6540         assertNull(calibrator.getEstimatedMzy());
6541         assertNull(calibrator.getEstimatedCovariance());
6542         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6543         assertNull(calibrator.getGroundTruthGravityNorm());
6544         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6545         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6546 
6547         // Force IllegalArgumentException
6548         calibrator = null;
6549         try {
6550             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6551                     new double[1], this);
6552             fail("IllegalArgumentException expected but not thrown");
6553         } catch (final IllegalArgumentException ignore) {
6554         }
6555         assertNull(calibrator);
6556     }
6557 
6558     @Test
6559     public void testConstructor59() throws WrongSizeException {
6560         final Matrix ba = generateBa();
6561         final double[] bias = ba.getBuffer();
6562         final double biasX = ba.getElementAtIndex(0);
6563         final double biasY = ba.getElementAtIndex(1);
6564         final double biasZ = ba.getElementAtIndex(2);
6565 
6566         KnownPositionAccelerometerCalibrator calibrator =
6567                 new KnownPositionAccelerometerCalibrator(true, bias);
6568 
6569         // check default values
6570         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6571         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6572         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6573         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6574         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6575         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6576         final Acceleration bx2 = new Acceleration(0.0,
6577                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6578         calibrator.getInitialBiasXAsAcceleration(bx2);
6579         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6580         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6581         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6582         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6583         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6584         final Acceleration by2 = new Acceleration(0.0,
6585                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6586         calibrator.getInitialBiasYAsAcceleration(by2);
6587         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6588         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6589         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6590         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6591         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6592         final Acceleration bz2 = new Acceleration(0.0,
6593                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6594         calibrator.getInitialBiasZAsAcceleration(bz2);
6595         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6596         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6597         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6598         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6599         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6600         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6601         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6602         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6603         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6604         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6605         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6606         final double[] bias1 = calibrator.getInitialBias();
6607         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6608         final double[] bias2 = new double[3];
6609         calibrator.getInitialBias(bias2);
6610         assertArrayEquals(bias1, bias2, 0.0);
6611         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6612         assertEquals(b1, ba);
6613         final Matrix b2 = new Matrix(3, 1);
6614         calibrator.getInitialBiasAsMatrix(b2);
6615         assertEquals(b1, b2);
6616         final Matrix ma1 = calibrator.getInitialMa();
6617         assertEquals(ma1, new Matrix(3, 3));
6618         final Matrix ma2 = new Matrix(3, 3);
6619         calibrator.getInitialMa(ma2);
6620         assertEquals(ma1, ma2);
6621         assertNull(calibrator.getEcefPosition());
6622         assertNull(calibrator.getNedPosition());
6623         assertFalse(calibrator.getNedPosition(null));
6624         assertNull(calibrator.getMeasurements());
6625         assertTrue(calibrator.isCommonAxisUsed());
6626         assertNull(calibrator.getListener());
6627         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6628         assertFalse(calibrator.isReady());
6629         assertFalse(calibrator.isRunning());
6630         assertNull(calibrator.getEstimatedBiases());
6631         assertFalse(calibrator.getEstimatedBiases(null));
6632         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6633         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6634         assertNull(calibrator.getEstimatedBiasFx());
6635         assertNull(calibrator.getEstimatedBiasFy());
6636         assertNull(calibrator.getEstimatedBiasFz());
6637         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6638         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6639         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6640         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6641         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6642         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6643         assertNull(calibrator.getEstimatedMa());
6644         assertNull(calibrator.getEstimatedSx());
6645         assertNull(calibrator.getEstimatedSy());
6646         assertNull(calibrator.getEstimatedSz());
6647         assertNull(calibrator.getEstimatedMxy());
6648         assertNull(calibrator.getEstimatedMxz());
6649         assertNull(calibrator.getEstimatedMyx());
6650         assertNull(calibrator.getEstimatedMyz());
6651         assertNull(calibrator.getEstimatedMzx());
6652         assertNull(calibrator.getEstimatedMzy());
6653         assertNull(calibrator.getEstimatedCovariance());
6654         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6655         assertNull(calibrator.getGroundTruthGravityNorm());
6656         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6657         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6658 
6659         // Force IllegalArgumentException
6660         calibrator = null;
6661         try {
6662             calibrator = new KnownPositionAccelerometerCalibrator(true,
6663                     new double[1]);
6664             fail("IllegalArgumentException expected but not thrown");
6665         } catch (final IllegalArgumentException ignore) {
6666         }
6667         assertNull(calibrator);
6668     }
6669 
6670     @Test
6671     public void testConstructor60() throws WrongSizeException {
6672         final Matrix ba = generateBa();
6673         final double[] bias = ba.getBuffer();
6674         final double biasX = ba.getElementAtIndex(0);
6675         final double biasY = ba.getElementAtIndex(1);
6676         final double biasZ = ba.getElementAtIndex(2);
6677 
6678         KnownPositionAccelerometerCalibrator calibrator =
6679                 new KnownPositionAccelerometerCalibrator(true, bias, this);
6680 
6681         // check default values
6682         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6683         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6684         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6685         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6686         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6687         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6688         final Acceleration bx2 = new Acceleration(0.0,
6689                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6690         calibrator.getInitialBiasXAsAcceleration(bx2);
6691         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6692         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6693         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6694         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6695         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6696         final Acceleration by2 = new Acceleration(0.0,
6697                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6698         calibrator.getInitialBiasYAsAcceleration(by2);
6699         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6700         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6701         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6702         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6703         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6704         final Acceleration bz2 = new Acceleration(0.0,
6705                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6706         calibrator.getInitialBiasZAsAcceleration(bz2);
6707         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6708         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6709         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6710         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6711         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6712         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6713         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6714         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6715         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6716         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6717         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6718         final double[] bias1 = calibrator.getInitialBias();
6719         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6720         final double[] bias2 = new double[3];
6721         calibrator.getInitialBias(bias2);
6722         assertArrayEquals(bias1, bias2, 0.0);
6723         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6724         assertEquals(b1, ba);
6725         final Matrix b2 = new Matrix(3, 1);
6726         calibrator.getInitialBiasAsMatrix(b2);
6727         assertEquals(b1, b2);
6728         final Matrix ma1 = calibrator.getInitialMa();
6729         assertEquals(ma1, new Matrix(3, 3));
6730         final Matrix ma2 = new Matrix(3, 3);
6731         calibrator.getInitialMa(ma2);
6732         assertEquals(ma1, ma2);
6733         assertNull(calibrator.getEcefPosition());
6734         assertNull(calibrator.getNedPosition());
6735         assertFalse(calibrator.getNedPosition(null));
6736         assertNull(calibrator.getMeasurements());
6737         assertTrue(calibrator.isCommonAxisUsed());
6738         assertSame(calibrator.getListener(), this);
6739         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6740         assertFalse(calibrator.isReady());
6741         assertFalse(calibrator.isRunning());
6742         assertNull(calibrator.getEstimatedBiases());
6743         assertFalse(calibrator.getEstimatedBiases(null));
6744         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6745         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6746         assertNull(calibrator.getEstimatedBiasFx());
6747         assertNull(calibrator.getEstimatedBiasFy());
6748         assertNull(calibrator.getEstimatedBiasFz());
6749         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6750         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6751         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6752         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6753         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6754         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6755         assertNull(calibrator.getEstimatedMa());
6756         assertNull(calibrator.getEstimatedSx());
6757         assertNull(calibrator.getEstimatedSy());
6758         assertNull(calibrator.getEstimatedSz());
6759         assertNull(calibrator.getEstimatedMxy());
6760         assertNull(calibrator.getEstimatedMxz());
6761         assertNull(calibrator.getEstimatedMyx());
6762         assertNull(calibrator.getEstimatedMyz());
6763         assertNull(calibrator.getEstimatedMzx());
6764         assertNull(calibrator.getEstimatedMzy());
6765         assertNull(calibrator.getEstimatedCovariance());
6766         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6767         assertNull(calibrator.getGroundTruthGravityNorm());
6768         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6769         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6770 
6771         // Force IllegalArgumentException
6772         calibrator = null;
6773         try {
6774             calibrator = new KnownPositionAccelerometerCalibrator(true,
6775                     new double[1], this);
6776             fail("IllegalArgumentException expected but not thrown");
6777         } catch (final IllegalArgumentException ignore) {
6778         }
6779         assertNull(calibrator);
6780     }
6781 
6782     @Test
6783     public void testConstructor61() throws WrongSizeException {
6784         final Collection<StandardDeviationBodyKinematics> measurements =
6785                 Collections.emptyList();
6786 
6787         final Matrix ba = generateBa();
6788         final double[] bias = ba.getBuffer();
6789         final double biasX = ba.getElementAtIndex(0);
6790         final double biasY = ba.getElementAtIndex(1);
6791         final double biasZ = ba.getElementAtIndex(2);
6792 
6793         KnownPositionAccelerometerCalibrator calibrator =
6794                 new KnownPositionAccelerometerCalibrator(measurements,
6795                         true, bias);
6796 
6797         // check default values
6798         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6799         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6800         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6801         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6802         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6803         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6804         final Acceleration bx2 = new Acceleration(0.0,
6805                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6806         calibrator.getInitialBiasXAsAcceleration(bx2);
6807         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6808         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6809         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6810         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6811         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6812         final Acceleration by2 = new Acceleration(0.0,
6813                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6814         calibrator.getInitialBiasYAsAcceleration(by2);
6815         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6816         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6817         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6818         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6819         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6820         final Acceleration bz2 = new Acceleration(0.0,
6821                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6822         calibrator.getInitialBiasZAsAcceleration(bz2);
6823         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6824         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6825         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6826         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6827         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6828         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6829         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6830         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6831         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6832         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6833         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6834         final double[] bias1 = calibrator.getInitialBias();
6835         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6836         final double[] bias2 = new double[3];
6837         calibrator.getInitialBias(bias2);
6838         assertArrayEquals(bias1, bias2, 0.0);
6839         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6840         assertEquals(b1, ba);
6841         final Matrix b2 = new Matrix(3, 1);
6842         calibrator.getInitialBiasAsMatrix(b2);
6843         assertEquals(b1, b2);
6844         final Matrix ma1 = calibrator.getInitialMa();
6845         assertEquals(ma1, new Matrix(3, 3));
6846         final Matrix ma2 = new Matrix(3, 3);
6847         calibrator.getInitialMa(ma2);
6848         assertEquals(ma1, ma2);
6849         assertNull(calibrator.getEcefPosition());
6850         assertNull(calibrator.getNedPosition());
6851         assertFalse(calibrator.getNedPosition(null));
6852         assertSame(calibrator.getMeasurements(), measurements);
6853         assertTrue(calibrator.isCommonAxisUsed());
6854         assertNull(calibrator.getListener());
6855         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6856         assertFalse(calibrator.isReady());
6857         assertFalse(calibrator.isRunning());
6858         assertNull(calibrator.getEstimatedBiases());
6859         assertFalse(calibrator.getEstimatedBiases(null));
6860         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6861         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6862         assertNull(calibrator.getEstimatedBiasFx());
6863         assertNull(calibrator.getEstimatedBiasFy());
6864         assertNull(calibrator.getEstimatedBiasFz());
6865         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6866         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6867         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6868         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6869         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6870         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6871         assertNull(calibrator.getEstimatedMa());
6872         assertNull(calibrator.getEstimatedSx());
6873         assertNull(calibrator.getEstimatedSy());
6874         assertNull(calibrator.getEstimatedSz());
6875         assertNull(calibrator.getEstimatedMxy());
6876         assertNull(calibrator.getEstimatedMxz());
6877         assertNull(calibrator.getEstimatedMyx());
6878         assertNull(calibrator.getEstimatedMyz());
6879         assertNull(calibrator.getEstimatedMzx());
6880         assertNull(calibrator.getEstimatedMzy());
6881         assertNull(calibrator.getEstimatedCovariance());
6882         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6883         assertNull(calibrator.getGroundTruthGravityNorm());
6884         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6885         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6886 
6887         // Force IllegalArgumentException
6888         calibrator = null;
6889         try {
6890             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6891                     true, new double[1]);
6892             fail("IllegalArgumentException expected but not thrown");
6893         } catch (final IllegalArgumentException ignore) {
6894         }
6895         assertNull(calibrator);
6896     }
6897 
6898     @Test
6899     public void testConstructor62() throws WrongSizeException {
6900         final Collection<StandardDeviationBodyKinematics> measurements =
6901                 Collections.emptyList();
6902 
6903         final Matrix ba = generateBa();
6904         final double[] bias = ba.getBuffer();
6905         final double biasX = ba.getElementAtIndex(0);
6906         final double biasY = ba.getElementAtIndex(1);
6907         final double biasZ = ba.getElementAtIndex(2);
6908 
6909         KnownPositionAccelerometerCalibrator calibrator =
6910                 new KnownPositionAccelerometerCalibrator(measurements,
6911                         true, bias, this);
6912 
6913         // check default values
6914         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6915         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6916         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6917         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6918         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6919         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6920         final Acceleration bx2 = new Acceleration(0.0,
6921                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6922         calibrator.getInitialBiasXAsAcceleration(bx2);
6923         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6924         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6925         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6926         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6927         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6928         final Acceleration by2 = new Acceleration(0.0,
6929                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6930         calibrator.getInitialBiasYAsAcceleration(by2);
6931         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6932         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6933         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6934         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6935         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6936         final Acceleration bz2 = new Acceleration(0.0,
6937                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6938         calibrator.getInitialBiasZAsAcceleration(bz2);
6939         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6940         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6941         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6942         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6943         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6944         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6945         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6946         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6947         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6948         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6949         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6950         final double[] bias1 = calibrator.getInitialBias();
6951         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6952         final double[] bias2 = new double[3];
6953         calibrator.getInitialBias(bias2);
6954         assertArrayEquals(bias1, bias2, 0.0);
6955         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6956         assertEquals(b1, ba);
6957         final Matrix b2 = new Matrix(3, 1);
6958         calibrator.getInitialBiasAsMatrix(b2);
6959         assertEquals(b1, b2);
6960         final Matrix ma1 = calibrator.getInitialMa();
6961         assertEquals(ma1, new Matrix(3, 3));
6962         final Matrix ma2 = new Matrix(3, 3);
6963         calibrator.getInitialMa(ma2);
6964         assertEquals(ma1, ma2);
6965         assertNull(calibrator.getEcefPosition());
6966         assertNull(calibrator.getNedPosition());
6967         assertFalse(calibrator.getNedPosition(null));
6968         assertSame(calibrator.getMeasurements(), measurements);
6969         assertTrue(calibrator.isCommonAxisUsed());
6970         assertSame(calibrator.getListener(), this);
6971         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6972         assertFalse(calibrator.isReady());
6973         assertFalse(calibrator.isRunning());
6974         assertNull(calibrator.getEstimatedBiases());
6975         assertFalse(calibrator.getEstimatedBiases(null));
6976         assertNull(calibrator.getEstimatedBiasesAsMatrix());
6977         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6978         assertNull(calibrator.getEstimatedBiasFx());
6979         assertNull(calibrator.getEstimatedBiasFy());
6980         assertNull(calibrator.getEstimatedBiasFz());
6981         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6982         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6983         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6984         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6985         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6986         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6987         assertNull(calibrator.getEstimatedMa());
6988         assertNull(calibrator.getEstimatedSx());
6989         assertNull(calibrator.getEstimatedSy());
6990         assertNull(calibrator.getEstimatedSz());
6991         assertNull(calibrator.getEstimatedMxy());
6992         assertNull(calibrator.getEstimatedMxz());
6993         assertNull(calibrator.getEstimatedMyx());
6994         assertNull(calibrator.getEstimatedMyz());
6995         assertNull(calibrator.getEstimatedMzx());
6996         assertNull(calibrator.getEstimatedMzy());
6997         assertNull(calibrator.getEstimatedCovariance());
6998         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6999         assertNull(calibrator.getGroundTruthGravityNorm());
7000         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7001         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7002 
7003         // Force IllegalArgumentException
7004         calibrator = null;
7005         try {
7006             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7007                     true, new double[1], this);
7008             fail("IllegalArgumentException expected but not thrown");
7009         } catch (final IllegalArgumentException ignore) {
7010         }
7011         assertNull(calibrator);
7012     }
7013 
7014     @Test
7015     public void testConstructor63() throws WrongSizeException {
7016         final Matrix ba = generateBa();
7017         final double biasX = ba.getElementAtIndex(0);
7018         final double biasY = ba.getElementAtIndex(1);
7019         final double biasZ = ba.getElementAtIndex(2);
7020 
7021         KnownPositionAccelerometerCalibrator calibrator =
7022                 new KnownPositionAccelerometerCalibrator(ba);
7023 
7024         // check default values
7025         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7026         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7027         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7028         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7029         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7030         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7031         final Acceleration bx2 = new Acceleration(0.0,
7032                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7033         calibrator.getInitialBiasXAsAcceleration(bx2);
7034         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7035         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7036         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7037         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7038         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7039         final Acceleration by2 = new Acceleration(0.0,
7040                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7041         calibrator.getInitialBiasYAsAcceleration(by2);
7042         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7043         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7044         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7045         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7046         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7047         final Acceleration bz2 = new Acceleration(0.0,
7048                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7049         calibrator.getInitialBiasZAsAcceleration(bz2);
7050         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7051         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7052         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7053         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7054         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7055         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7056         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7057         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7058         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7059         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7060         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7061         final double[] bias1 = calibrator.getInitialBias();
7062         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7063         final double[] bias2 = new double[3];
7064         calibrator.getInitialBias(bias2);
7065         assertArrayEquals(bias1, bias2, 0.0);
7066         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7067         assertEquals(b1, ba);
7068         final Matrix b2 = new Matrix(3, 1);
7069         calibrator.getInitialBiasAsMatrix(b2);
7070         assertEquals(b1, b2);
7071         final Matrix ma1 = calibrator.getInitialMa();
7072         assertEquals(ma1, new Matrix(3, 3));
7073         final Matrix ma2 = new Matrix(3, 3);
7074         calibrator.getInitialMa(ma2);
7075         assertEquals(ma1, ma2);
7076         assertNull(calibrator.getEcefPosition());
7077         assertNull(calibrator.getNedPosition());
7078         assertFalse(calibrator.getNedPosition(null));
7079         assertNull(calibrator.getMeasurements());
7080         assertFalse(calibrator.isCommonAxisUsed());
7081         assertNull(calibrator.getListener());
7082         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7083         assertFalse(calibrator.isReady());
7084         assertFalse(calibrator.isRunning());
7085         assertNull(calibrator.getEstimatedBiases());
7086         assertFalse(calibrator.getEstimatedBiases(null));
7087         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7088         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7089         assertNull(calibrator.getEstimatedBiasFx());
7090         assertNull(calibrator.getEstimatedBiasFy());
7091         assertNull(calibrator.getEstimatedBiasFz());
7092         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7093         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7094         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7095         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7096         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7097         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7098         assertNull(calibrator.getEstimatedMa());
7099         assertNull(calibrator.getEstimatedSx());
7100         assertNull(calibrator.getEstimatedSy());
7101         assertNull(calibrator.getEstimatedSz());
7102         assertNull(calibrator.getEstimatedMxy());
7103         assertNull(calibrator.getEstimatedMxz());
7104         assertNull(calibrator.getEstimatedMyx());
7105         assertNull(calibrator.getEstimatedMyz());
7106         assertNull(calibrator.getEstimatedMzx());
7107         assertNull(calibrator.getEstimatedMzy());
7108         assertNull(calibrator.getEstimatedCovariance());
7109         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7110         assertNull(calibrator.getGroundTruthGravityNorm());
7111         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7112         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7113 
7114         // Force IllegalArgumentException
7115         calibrator = null;
7116         try {
7117             calibrator = new KnownPositionAccelerometerCalibrator(
7118                     new Matrix(1, 1));
7119             fail("IllegalArgumentException expected but not thrown");
7120         } catch (final IllegalArgumentException ignore) {
7121         }
7122         try {
7123             calibrator = new KnownPositionAccelerometerCalibrator(
7124                     new Matrix(1, 3));
7125             fail("IllegalArgumentException expected but not thrown");
7126         } catch (final IllegalArgumentException ignore) {
7127         }
7128         assertNull(calibrator);
7129     }
7130 
7131     @Test
7132     public void testConstructor64() throws WrongSizeException {
7133         final Matrix ba = generateBa();
7134         final double biasX = ba.getElementAtIndex(0);
7135         final double biasY = ba.getElementAtIndex(1);
7136         final double biasZ = ba.getElementAtIndex(2);
7137 
7138         KnownPositionAccelerometerCalibrator calibrator =
7139                 new KnownPositionAccelerometerCalibrator(ba, this);
7140 
7141         // check default values
7142         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7143         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7144         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7145         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7146         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7147         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7148         final Acceleration bx2 = new Acceleration(0.0,
7149                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7150         calibrator.getInitialBiasXAsAcceleration(bx2);
7151         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7152         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7153         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7154         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7155         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7156         final Acceleration by2 = new Acceleration(0.0,
7157                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7158         calibrator.getInitialBiasYAsAcceleration(by2);
7159         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7160         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7161         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7162         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7163         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7164         final Acceleration bz2 = new Acceleration(0.0,
7165                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7166         calibrator.getInitialBiasZAsAcceleration(bz2);
7167         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7168         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7169         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7170         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7171         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7172         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7173         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7174         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7175         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7176         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7177         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7178         final double[] bias1 = calibrator.getInitialBias();
7179         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7180         final double[] bias2 = new double[3];
7181         calibrator.getInitialBias(bias2);
7182         assertArrayEquals(bias1, bias2, 0.0);
7183         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7184         assertEquals(b1, ba);
7185         final Matrix b2 = new Matrix(3, 1);
7186         calibrator.getInitialBiasAsMatrix(b2);
7187         assertEquals(b1, b2);
7188         final Matrix ma1 = calibrator.getInitialMa();
7189         assertEquals(ma1, new Matrix(3, 3));
7190         final Matrix ma2 = new Matrix(3, 3);
7191         calibrator.getInitialMa(ma2);
7192         assertEquals(ma1, ma2);
7193         assertNull(calibrator.getEcefPosition());
7194         assertNull(calibrator.getNedPosition());
7195         assertFalse(calibrator.getNedPosition(null));
7196         assertNull(calibrator.getMeasurements());
7197         assertFalse(calibrator.isCommonAxisUsed());
7198         assertSame(calibrator.getListener(), this);
7199         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7200         assertFalse(calibrator.isReady());
7201         assertFalse(calibrator.isRunning());
7202         assertNull(calibrator.getEstimatedBiases());
7203         assertFalse(calibrator.getEstimatedBiases(null));
7204         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7205         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7206         assertNull(calibrator.getEstimatedBiasFx());
7207         assertNull(calibrator.getEstimatedBiasFy());
7208         assertNull(calibrator.getEstimatedBiasFz());
7209         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7210         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7211         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7212         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7213         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7214         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7215         assertNull(calibrator.getEstimatedMa());
7216         assertNull(calibrator.getEstimatedSx());
7217         assertNull(calibrator.getEstimatedSy());
7218         assertNull(calibrator.getEstimatedSz());
7219         assertNull(calibrator.getEstimatedMxy());
7220         assertNull(calibrator.getEstimatedMxz());
7221         assertNull(calibrator.getEstimatedMyx());
7222         assertNull(calibrator.getEstimatedMyz());
7223         assertNull(calibrator.getEstimatedMzx());
7224         assertNull(calibrator.getEstimatedMzy());
7225         assertNull(calibrator.getEstimatedCovariance());
7226         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7227         assertNull(calibrator.getGroundTruthGravityNorm());
7228         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7229         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7230 
7231         // Force IllegalArgumentException
7232         calibrator = null;
7233         try {
7234             calibrator = new KnownPositionAccelerometerCalibrator(
7235                     new Matrix(1, 1));
7236             fail("IllegalArgumentException expected but not thrown");
7237         } catch (final IllegalArgumentException ignore) {
7238         }
7239         try {
7240             calibrator = new KnownPositionAccelerometerCalibrator(
7241                     new Matrix(1, 3));
7242             fail("IllegalArgumentException expected but not thrown");
7243         } catch (final IllegalArgumentException ignore) {
7244         }
7245         assertNull(calibrator);
7246     }
7247 
7248     @Test
7249     public void testConstructor65() throws WrongSizeException {
7250         final Collection<StandardDeviationBodyKinematics> measurements =
7251                 Collections.emptyList();
7252 
7253         final Matrix ba = generateBa();
7254         final double biasX = ba.getElementAtIndex(0);
7255         final double biasY = ba.getElementAtIndex(1);
7256         final double biasZ = ba.getElementAtIndex(2);
7257 
7258         KnownPositionAccelerometerCalibrator calibrator =
7259                 new KnownPositionAccelerometerCalibrator(measurements, ba);
7260 
7261         // check default values
7262         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7263         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7264         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7265         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7266         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7267         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7268         final Acceleration bx2 = new Acceleration(0.0,
7269                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7270         calibrator.getInitialBiasXAsAcceleration(bx2);
7271         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7272         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7273         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7274         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7275         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7276         final Acceleration by2 = new Acceleration(0.0,
7277                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7278         calibrator.getInitialBiasYAsAcceleration(by2);
7279         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7280         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7281         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7282         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7283         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7284         final Acceleration bz2 = new Acceleration(0.0,
7285                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7286         calibrator.getInitialBiasZAsAcceleration(bz2);
7287         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7288         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7289         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7290         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7291         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7292         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7293         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7294         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7295         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7296         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7297         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7298         final double[] bias1 = calibrator.getInitialBias();
7299         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7300         final double[] bias2 = new double[3];
7301         calibrator.getInitialBias(bias2);
7302         assertArrayEquals(bias1, bias2, 0.0);
7303         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7304         assertEquals(b1, ba);
7305         final Matrix b2 = new Matrix(3, 1);
7306         calibrator.getInitialBiasAsMatrix(b2);
7307         assertEquals(b1, b2);
7308         final Matrix ma1 = calibrator.getInitialMa();
7309         assertEquals(ma1, new Matrix(3, 3));
7310         final Matrix ma2 = new Matrix(3, 3);
7311         calibrator.getInitialMa(ma2);
7312         assertEquals(ma1, ma2);
7313         assertNull(calibrator.getEcefPosition());
7314         assertNull(calibrator.getNedPosition());
7315         assertFalse(calibrator.getNedPosition(null));
7316         assertSame(calibrator.getMeasurements(), measurements);
7317         assertFalse(calibrator.isCommonAxisUsed());
7318         assertNull(calibrator.getListener());
7319         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7320         assertFalse(calibrator.isReady());
7321         assertFalse(calibrator.isRunning());
7322         assertNull(calibrator.getEstimatedBiases());
7323         assertFalse(calibrator.getEstimatedBiases(null));
7324         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7325         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7326         assertNull(calibrator.getEstimatedBiasFx());
7327         assertNull(calibrator.getEstimatedBiasFy());
7328         assertNull(calibrator.getEstimatedBiasFz());
7329         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7330         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7331         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7332         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7333         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7334         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7335         assertNull(calibrator.getEstimatedMa());
7336         assertNull(calibrator.getEstimatedSx());
7337         assertNull(calibrator.getEstimatedSy());
7338         assertNull(calibrator.getEstimatedSz());
7339         assertNull(calibrator.getEstimatedMxy());
7340         assertNull(calibrator.getEstimatedMxz());
7341         assertNull(calibrator.getEstimatedMyx());
7342         assertNull(calibrator.getEstimatedMyz());
7343         assertNull(calibrator.getEstimatedMzx());
7344         assertNull(calibrator.getEstimatedMzy());
7345         assertNull(calibrator.getEstimatedCovariance());
7346         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7347         assertNull(calibrator.getGroundTruthGravityNorm());
7348         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7349         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7350 
7351         // Force IllegalArgumentException
7352         calibrator = null;
7353         try {
7354             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7355                     new Matrix(1, 1));
7356             fail("IllegalArgumentException expected but not thrown");
7357         } catch (final IllegalArgumentException ignore) {
7358         }
7359         try {
7360             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7361                     new Matrix(1, 3));
7362             fail("IllegalArgumentException expected but not thrown");
7363         } catch (final IllegalArgumentException ignore) {
7364         }
7365         assertNull(calibrator);
7366     }
7367 
7368     @Test
7369     public void testConstructor66() throws WrongSizeException {
7370         final Collection<StandardDeviationBodyKinematics> measurements =
7371                 Collections.emptyList();
7372 
7373         final Matrix ba = generateBa();
7374         final double biasX = ba.getElementAtIndex(0);
7375         final double biasY = ba.getElementAtIndex(1);
7376         final double biasZ = ba.getElementAtIndex(2);
7377 
7378         KnownPositionAccelerometerCalibrator calibrator =
7379                 new KnownPositionAccelerometerCalibrator(measurements, ba,
7380                         this);
7381 
7382         // check default values
7383         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7384         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7385         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7386         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7387         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7388         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7389         final Acceleration bx2 = new Acceleration(0.0,
7390                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7391         calibrator.getInitialBiasXAsAcceleration(bx2);
7392         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7393         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7394         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7395         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7396         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7397         final Acceleration by2 = new Acceleration(0.0,
7398                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7399         calibrator.getInitialBiasYAsAcceleration(by2);
7400         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7401         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7402         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7403         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7404         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7405         final Acceleration bz2 = new Acceleration(0.0,
7406                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7407         calibrator.getInitialBiasZAsAcceleration(bz2);
7408         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7409         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7410         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7411         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7412         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7413         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7414         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7415         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7416         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7417         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7418         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7419         final double[] bias1 = calibrator.getInitialBias();
7420         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7421         final double[] bias2 = new double[3];
7422         calibrator.getInitialBias(bias2);
7423         assertArrayEquals(bias1, bias2, 0.0);
7424         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7425         assertEquals(b1, ba);
7426         final Matrix b2 = new Matrix(3, 1);
7427         calibrator.getInitialBiasAsMatrix(b2);
7428         assertEquals(b1, b2);
7429         final Matrix ma1 = calibrator.getInitialMa();
7430         assertEquals(ma1, new Matrix(3, 3));
7431         final Matrix ma2 = new Matrix(3, 3);
7432         calibrator.getInitialMa(ma2);
7433         assertEquals(ma1, ma2);
7434         assertNull(calibrator.getEcefPosition());
7435         assertNull(calibrator.getNedPosition());
7436         assertFalse(calibrator.getNedPosition(null));
7437         assertSame(calibrator.getMeasurements(), measurements);
7438         assertFalse(calibrator.isCommonAxisUsed());
7439         assertSame(calibrator.getListener(), this);
7440         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7441         assertFalse(calibrator.isReady());
7442         assertFalse(calibrator.isRunning());
7443         assertNull(calibrator.getEstimatedBiases());
7444         assertFalse(calibrator.getEstimatedBiases(null));
7445         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7446         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7447         assertNull(calibrator.getEstimatedBiasFx());
7448         assertNull(calibrator.getEstimatedBiasFy());
7449         assertNull(calibrator.getEstimatedBiasFz());
7450         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7451         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7452         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7453         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7454         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7455         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7456         assertNull(calibrator.getEstimatedMa());
7457         assertNull(calibrator.getEstimatedSx());
7458         assertNull(calibrator.getEstimatedSy());
7459         assertNull(calibrator.getEstimatedSz());
7460         assertNull(calibrator.getEstimatedMxy());
7461         assertNull(calibrator.getEstimatedMxz());
7462         assertNull(calibrator.getEstimatedMyx());
7463         assertNull(calibrator.getEstimatedMyz());
7464         assertNull(calibrator.getEstimatedMzx());
7465         assertNull(calibrator.getEstimatedMzy());
7466         assertNull(calibrator.getEstimatedCovariance());
7467         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7468         assertNull(calibrator.getGroundTruthGravityNorm());
7469         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7470         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7471 
7472         // Force IllegalArgumentException
7473         calibrator = null;
7474         try {
7475             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7476                     new Matrix(1, 1), this);
7477             fail("IllegalArgumentException expected but not thrown");
7478         } catch (final IllegalArgumentException ignore) {
7479         }
7480         try {
7481             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7482                     new Matrix(1, 3), this);
7483             fail("IllegalArgumentException expected but not thrown");
7484         } catch (final IllegalArgumentException ignore) {
7485         }
7486         assertNull(calibrator);
7487     }
7488 
7489     @Test
7490     public void testConstructor67() throws WrongSizeException {
7491         final Matrix ba = generateBa();
7492         final double biasX = ba.getElementAtIndex(0);
7493         final double biasY = ba.getElementAtIndex(1);
7494         final double biasZ = ba.getElementAtIndex(2);
7495 
7496         KnownPositionAccelerometerCalibrator calibrator =
7497                 new KnownPositionAccelerometerCalibrator(true, ba);
7498 
7499         // check default values
7500         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7501         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7502         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7503         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7504         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7505         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7506         final Acceleration bx2 = new Acceleration(0.0,
7507                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7508         calibrator.getInitialBiasXAsAcceleration(bx2);
7509         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7510         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7511         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7512         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7513         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7514         final Acceleration by2 = new Acceleration(0.0,
7515                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7516         calibrator.getInitialBiasYAsAcceleration(by2);
7517         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7518         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7519         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7520         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7521         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7522         final Acceleration bz2 = new Acceleration(0.0,
7523                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7524         calibrator.getInitialBiasZAsAcceleration(bz2);
7525         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7526         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7527         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7528         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7529         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7530         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7531         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7532         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7533         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7534         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7535         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7536         final double[] bias1 = calibrator.getInitialBias();
7537         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7538         final double[] bias2 = new double[3];
7539         calibrator.getInitialBias(bias2);
7540         assertArrayEquals(bias1, bias2, 0.0);
7541         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7542         assertEquals(b1, ba);
7543         final Matrix b2 = new Matrix(3, 1);
7544         calibrator.getInitialBiasAsMatrix(b2);
7545         assertEquals(b1, b2);
7546         final Matrix ma1 = calibrator.getInitialMa();
7547         assertEquals(ma1, new Matrix(3, 3));
7548         final Matrix ma2 = new Matrix(3, 3);
7549         calibrator.getInitialMa(ma2);
7550         assertEquals(ma1, ma2);
7551         assertNull(calibrator.getEcefPosition());
7552         assertNull(calibrator.getNedPosition());
7553         assertFalse(calibrator.getNedPosition(null));
7554         assertNull(calibrator.getMeasurements());
7555         assertTrue(calibrator.isCommonAxisUsed());
7556         assertNull(calibrator.getListener());
7557         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7558         assertFalse(calibrator.isReady());
7559         assertFalse(calibrator.isRunning());
7560         assertNull(calibrator.getEstimatedBiases());
7561         assertFalse(calibrator.getEstimatedBiases(null));
7562         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7563         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7564         assertNull(calibrator.getEstimatedBiasFx());
7565         assertNull(calibrator.getEstimatedBiasFy());
7566         assertNull(calibrator.getEstimatedBiasFz());
7567         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7568         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7569         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7570         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7571         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7572         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7573         assertNull(calibrator.getEstimatedMa());
7574         assertNull(calibrator.getEstimatedSx());
7575         assertNull(calibrator.getEstimatedSy());
7576         assertNull(calibrator.getEstimatedSz());
7577         assertNull(calibrator.getEstimatedMxy());
7578         assertNull(calibrator.getEstimatedMxz());
7579         assertNull(calibrator.getEstimatedMyx());
7580         assertNull(calibrator.getEstimatedMyz());
7581         assertNull(calibrator.getEstimatedMzx());
7582         assertNull(calibrator.getEstimatedMzy());
7583         assertNull(calibrator.getEstimatedCovariance());
7584         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7585         assertNull(calibrator.getGroundTruthGravityNorm());
7586         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7587         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7588 
7589         // Force IllegalArgumentException
7590         calibrator = null;
7591         try {
7592             calibrator = new KnownPositionAccelerometerCalibrator(
7593                     true, new Matrix(1, 1));
7594             fail("IllegalArgumentException expected but not thrown");
7595         } catch (final IllegalArgumentException ignore) {
7596         }
7597         try {
7598             calibrator = new KnownPositionAccelerometerCalibrator(
7599                     true, new Matrix(1, 3));
7600             fail("IllegalArgumentException expected but not thrown");
7601         } catch (final IllegalArgumentException ignore) {
7602         }
7603         assertNull(calibrator);
7604     }
7605 
7606     @Test
7607     public void testConstructor68() throws WrongSizeException {
7608         final Matrix ba = generateBa();
7609         final double biasX = ba.getElementAtIndex(0);
7610         final double biasY = ba.getElementAtIndex(1);
7611         final double biasZ = ba.getElementAtIndex(2);
7612 
7613         KnownPositionAccelerometerCalibrator calibrator =
7614                 new KnownPositionAccelerometerCalibrator(true, ba,
7615                         this);
7616 
7617         // check default values
7618         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7619         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7620         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7621         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7622         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7623         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7624         final Acceleration bx2 = new Acceleration(0.0,
7625                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7626         calibrator.getInitialBiasXAsAcceleration(bx2);
7627         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7628         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7629         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7630         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7631         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7632         final Acceleration by2 = new Acceleration(0.0,
7633                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7634         calibrator.getInitialBiasYAsAcceleration(by2);
7635         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7636         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7637         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7638         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7639         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7640         final Acceleration bz2 = new Acceleration(0.0,
7641                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7642         calibrator.getInitialBiasZAsAcceleration(bz2);
7643         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7644         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7645         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7646         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7647         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7648         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7649         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7650         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7651         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7652         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7653         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7654         final double[] bias1 = calibrator.getInitialBias();
7655         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7656         final double[] bias2 = new double[3];
7657         calibrator.getInitialBias(bias2);
7658         assertArrayEquals(bias1, bias2, 0.0);
7659         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7660         assertEquals(b1, ba);
7661         final Matrix b2 = new Matrix(3, 1);
7662         calibrator.getInitialBiasAsMatrix(b2);
7663         assertEquals(b1, b2);
7664         final Matrix ma1 = calibrator.getInitialMa();
7665         assertEquals(ma1, new Matrix(3, 3));
7666         final Matrix ma2 = new Matrix(3, 3);
7667         calibrator.getInitialMa(ma2);
7668         assertEquals(ma1, ma2);
7669         assertNull(calibrator.getEcefPosition());
7670         assertNull(calibrator.getNedPosition());
7671         assertFalse(calibrator.getNedPosition(null));
7672         assertNull(calibrator.getMeasurements());
7673         assertTrue(calibrator.isCommonAxisUsed());
7674         assertSame(calibrator.getListener(), this);
7675         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7676         assertFalse(calibrator.isReady());
7677         assertFalse(calibrator.isRunning());
7678         assertNull(calibrator.getEstimatedBiases());
7679         assertFalse(calibrator.getEstimatedBiases(null));
7680         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7681         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7682         assertNull(calibrator.getEstimatedBiasFx());
7683         assertNull(calibrator.getEstimatedBiasFy());
7684         assertNull(calibrator.getEstimatedBiasFz());
7685         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7686         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7687         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7688         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7689         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7690         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7691         assertNull(calibrator.getEstimatedMa());
7692         assertNull(calibrator.getEstimatedSx());
7693         assertNull(calibrator.getEstimatedSy());
7694         assertNull(calibrator.getEstimatedSz());
7695         assertNull(calibrator.getEstimatedMxy());
7696         assertNull(calibrator.getEstimatedMxz());
7697         assertNull(calibrator.getEstimatedMyx());
7698         assertNull(calibrator.getEstimatedMyz());
7699         assertNull(calibrator.getEstimatedMzx());
7700         assertNull(calibrator.getEstimatedMzy());
7701         assertNull(calibrator.getEstimatedCovariance());
7702         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7703         assertNull(calibrator.getGroundTruthGravityNorm());
7704         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7705         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7706 
7707         // Force IllegalArgumentException
7708         calibrator = null;
7709         try {
7710             calibrator = new KnownPositionAccelerometerCalibrator(
7711                     true, new Matrix(1, 1),
7712                     this);
7713             fail("IllegalArgumentException expected but not thrown");
7714         } catch (final IllegalArgumentException ignore) {
7715         }
7716         try {
7717             calibrator = new KnownPositionAccelerometerCalibrator(
7718                     true, new Matrix(1, 3),
7719                     this);
7720             fail("IllegalArgumentException expected but not thrown");
7721         } catch (final IllegalArgumentException ignore) {
7722         }
7723         assertNull(calibrator);
7724     }
7725 
7726     @Test
7727     public void testConstructor69() throws WrongSizeException {
7728         final Collection<StandardDeviationBodyKinematics> measurements =
7729                 Collections.emptyList();
7730 
7731         final Matrix ba = generateBa();
7732         final double biasX = ba.getElementAtIndex(0);
7733         final double biasY = ba.getElementAtIndex(1);
7734         final double biasZ = ba.getElementAtIndex(2);
7735 
7736         KnownPositionAccelerometerCalibrator calibrator =
7737                 new KnownPositionAccelerometerCalibrator(measurements,
7738                         true, ba);
7739 
7740         // check default values
7741         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7742         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7743         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7744         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7745         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7746         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7747         final Acceleration bx2 = new Acceleration(0.0,
7748                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7749         calibrator.getInitialBiasXAsAcceleration(bx2);
7750         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7751         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7752         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7753         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7754         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7755         final Acceleration by2 = new Acceleration(0.0,
7756                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7757         calibrator.getInitialBiasYAsAcceleration(by2);
7758         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7759         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7760         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7761         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7762         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7763         final Acceleration bz2 = new Acceleration(0.0,
7764                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7765         calibrator.getInitialBiasZAsAcceleration(bz2);
7766         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7767         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7768         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7769         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7770         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7771         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7772         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7773         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7774         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7775         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7776         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7777         final double[] bias1 = calibrator.getInitialBias();
7778         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7779         final double[] bias2 = new double[3];
7780         calibrator.getInitialBias(bias2);
7781         assertArrayEquals(bias1, bias2, 0.0);
7782         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7783         assertEquals(b1, ba);
7784         final Matrix b2 = new Matrix(3, 1);
7785         calibrator.getInitialBiasAsMatrix(b2);
7786         assertEquals(b1, b2);
7787         final Matrix ma1 = calibrator.getInitialMa();
7788         assertEquals(ma1, new Matrix(3, 3));
7789         final Matrix ma2 = new Matrix(3, 3);
7790         calibrator.getInitialMa(ma2);
7791         assertEquals(ma1, ma2);
7792         assertNull(calibrator.getEcefPosition());
7793         assertNull(calibrator.getNedPosition());
7794         assertFalse(calibrator.getNedPosition(null));
7795         assertSame(calibrator.getMeasurements(), measurements);
7796         assertTrue(calibrator.isCommonAxisUsed());
7797         assertNull(calibrator.getListener());
7798         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7799         assertFalse(calibrator.isReady());
7800         assertFalse(calibrator.isRunning());
7801         assertNull(calibrator.getEstimatedBiases());
7802         assertFalse(calibrator.getEstimatedBiases(null));
7803         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7804         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7805         assertNull(calibrator.getEstimatedBiasFx());
7806         assertNull(calibrator.getEstimatedBiasFy());
7807         assertNull(calibrator.getEstimatedBiasFz());
7808         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7809         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7810         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7811         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7812         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7813         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7814         assertNull(calibrator.getEstimatedMa());
7815         assertNull(calibrator.getEstimatedSx());
7816         assertNull(calibrator.getEstimatedSy());
7817         assertNull(calibrator.getEstimatedSz());
7818         assertNull(calibrator.getEstimatedMxy());
7819         assertNull(calibrator.getEstimatedMxz());
7820         assertNull(calibrator.getEstimatedMyx());
7821         assertNull(calibrator.getEstimatedMyz());
7822         assertNull(calibrator.getEstimatedMzx());
7823         assertNull(calibrator.getEstimatedMzy());
7824         assertNull(calibrator.getEstimatedCovariance());
7825         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7826         assertNull(calibrator.getGroundTruthGravityNorm());
7827         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7828         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7829 
7830         // Force IllegalArgumentException
7831         calibrator = null;
7832         try {
7833             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7834                     true, new Matrix(1, 1));
7835             fail("IllegalArgumentException expected but not thrown");
7836         } catch (final IllegalArgumentException ignore) {
7837         }
7838         try {
7839             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7840                     true, new Matrix(1, 3));
7841             fail("IllegalArgumentException expected but not thrown");
7842         } catch (final IllegalArgumentException ignore) {
7843         }
7844         assertNull(calibrator);
7845     }
7846 
7847     @Test
7848     public void testConstructor70() throws WrongSizeException {
7849         final Collection<StandardDeviationBodyKinematics> measurements =
7850                 Collections.emptyList();
7851 
7852         final Matrix ba = generateBa();
7853         final double biasX = ba.getElementAtIndex(0);
7854         final double biasY = ba.getElementAtIndex(1);
7855         final double biasZ = ba.getElementAtIndex(2);
7856 
7857         KnownPositionAccelerometerCalibrator calibrator =
7858                 new KnownPositionAccelerometerCalibrator(measurements,
7859                         true, ba, this);
7860 
7861         // check default values
7862         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7863         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7864         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7865         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7866         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7867         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7868         final Acceleration bx2 = new Acceleration(0.0,
7869                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7870         calibrator.getInitialBiasXAsAcceleration(bx2);
7871         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7872         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7873         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7874         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7875         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7876         final Acceleration by2 = new Acceleration(0.0,
7877                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7878         calibrator.getInitialBiasYAsAcceleration(by2);
7879         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7880         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7881         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7882         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7883         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7884         final Acceleration bz2 = new Acceleration(0.0,
7885                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7886         calibrator.getInitialBiasZAsAcceleration(bz2);
7887         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7888         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7889         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7890         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7891         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7892         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7893         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7894         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7895         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7896         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7897         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7898         final double[] bias1 = calibrator.getInitialBias();
7899         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7900         final double[] bias2 = new double[3];
7901         calibrator.getInitialBias(bias2);
7902         assertArrayEquals(bias1, bias2, 0.0);
7903         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7904         assertEquals(b1, ba);
7905         final Matrix b2 = new Matrix(3, 1);
7906         calibrator.getInitialBiasAsMatrix(b2);
7907         assertEquals(b1, b2);
7908         final Matrix ma1 = calibrator.getInitialMa();
7909         assertEquals(ma1, new Matrix(3, 3));
7910         final Matrix ma2 = new Matrix(3, 3);
7911         calibrator.getInitialMa(ma2);
7912         assertEquals(ma1, ma2);
7913         assertNull(calibrator.getEcefPosition());
7914         assertNull(calibrator.getNedPosition());
7915         assertFalse(calibrator.getNedPosition(null));
7916         assertSame(calibrator.getMeasurements(), measurements);
7917         assertTrue(calibrator.isCommonAxisUsed());
7918         assertSame(calibrator.getListener(), this);
7919         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7920         assertFalse(calibrator.isReady());
7921         assertFalse(calibrator.isRunning());
7922         assertNull(calibrator.getEstimatedBiases());
7923         assertFalse(calibrator.getEstimatedBiases(null));
7924         assertNull(calibrator.getEstimatedBiasesAsMatrix());
7925         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7926         assertNull(calibrator.getEstimatedBiasFx());
7927         assertNull(calibrator.getEstimatedBiasFy());
7928         assertNull(calibrator.getEstimatedBiasFz());
7929         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7930         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7931         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7932         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7933         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7934         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7935         assertNull(calibrator.getEstimatedMa());
7936         assertNull(calibrator.getEstimatedSx());
7937         assertNull(calibrator.getEstimatedSy());
7938         assertNull(calibrator.getEstimatedSz());
7939         assertNull(calibrator.getEstimatedMxy());
7940         assertNull(calibrator.getEstimatedMxz());
7941         assertNull(calibrator.getEstimatedMyx());
7942         assertNull(calibrator.getEstimatedMyz());
7943         assertNull(calibrator.getEstimatedMzx());
7944         assertNull(calibrator.getEstimatedMzy());
7945         assertNull(calibrator.getEstimatedCovariance());
7946         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7947         assertNull(calibrator.getGroundTruthGravityNorm());
7948         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7949         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7950 
7951         // Force IllegalArgumentException
7952         calibrator = null;
7953         try {
7954             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7955                     true, new Matrix(1, 1),
7956                     this);
7957             fail("IllegalArgumentException expected but not thrown");
7958         } catch (final IllegalArgumentException ignore) {
7959         }
7960         try {
7961             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7962                     true, new Matrix(1, 3),
7963                     this);
7964             fail("IllegalArgumentException expected but not thrown");
7965         } catch (final IllegalArgumentException ignore) {
7966         }
7967         assertNull(calibrator);
7968     }
7969 
7970     @Test
7971     public void testConstructor71() throws WrongSizeException {
7972         final Matrix ba = generateBa();
7973         final double biasX = ba.getElementAtIndex(0);
7974         final double biasY = ba.getElementAtIndex(1);
7975         final double biasZ = ba.getElementAtIndex(2);
7976 
7977         final Matrix ma = generateMaCommonAxis();
7978         final double sx = ma.getElementAt(0, 0);
7979         final double sy = ma.getElementAt(1, 1);
7980         final double sz = ma.getElementAt(2, 2);
7981         final double mxy = ma.getElementAt(0, 1);
7982         final double mxz = ma.getElementAt(0, 2);
7983         final double myx = ma.getElementAt(1, 0);
7984         final double myz = ma.getElementAt(1, 2);
7985         final double mzx = ma.getElementAt(2, 0);
7986         final double mzy = ma.getElementAt(2, 1);
7987 
7988         KnownPositionAccelerometerCalibrator calibrator =
7989                 new KnownPositionAccelerometerCalibrator(ba, ma);
7990 
7991         // check default values
7992         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7993         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7994         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7995         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7996         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7997         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7998         final Acceleration bx2 = new Acceleration(0.0,
7999                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8000         calibrator.getInitialBiasXAsAcceleration(bx2);
8001         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8002         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8003         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8004         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8005         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8006         final Acceleration by2 = new Acceleration(0.0,
8007                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8008         calibrator.getInitialBiasYAsAcceleration(by2);
8009         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8010         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8011         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8012         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8013         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8014         final Acceleration bz2 = new Acceleration(0.0,
8015                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8016         calibrator.getInitialBiasZAsAcceleration(bz2);
8017         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8018         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8019         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8020         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8021         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8022         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8023         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8024         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8025         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8026         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8027         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8028         final double[] bias1 = calibrator.getInitialBias();
8029         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8030         final double[] bias2 = new double[3];
8031         calibrator.getInitialBias(bias2);
8032         assertArrayEquals(bias1, bias2, 0.0);
8033         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8034         assertEquals(b1, ba);
8035         final Matrix b2 = new Matrix(3, 1);
8036         calibrator.getInitialBiasAsMatrix(b2);
8037         assertEquals(b1, b2);
8038         final Matrix ma1 = new Matrix(3, 3);
8039         ma1.setSubmatrix(0, 0,
8040                 2, 2,
8041                 new double[]{sx, myx, mzx,
8042                         mxy, sy, mzy,
8043                         mxz, myz, sz});
8044         assertEquals(calibrator.getInitialMa(), ma1);
8045         final Matrix ma2 = new Matrix(3, 3);
8046         calibrator.getInitialMa(ma2);
8047         assertEquals(ma1, ma2);
8048         assertNull(calibrator.getEcefPosition());
8049         assertNull(calibrator.getNedPosition());
8050         assertFalse(calibrator.getNedPosition(null));
8051         assertNull(calibrator.getMeasurements());
8052         assertFalse(calibrator.isCommonAxisUsed());
8053         assertNull(calibrator.getListener());
8054         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8055         assertFalse(calibrator.isReady());
8056         assertFalse(calibrator.isRunning());
8057         assertNull(calibrator.getEstimatedBiases());
8058         assertFalse(calibrator.getEstimatedBiases(null));
8059         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8060         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8061         assertNull(calibrator.getEstimatedBiasFx());
8062         assertNull(calibrator.getEstimatedBiasFy());
8063         assertNull(calibrator.getEstimatedBiasFz());
8064         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8065         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8066         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8067         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8068         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8069         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8070         assertNull(calibrator.getEstimatedMa());
8071         assertNull(calibrator.getEstimatedSx());
8072         assertNull(calibrator.getEstimatedSy());
8073         assertNull(calibrator.getEstimatedSz());
8074         assertNull(calibrator.getEstimatedMxy());
8075         assertNull(calibrator.getEstimatedMxz());
8076         assertNull(calibrator.getEstimatedMyx());
8077         assertNull(calibrator.getEstimatedMyz());
8078         assertNull(calibrator.getEstimatedMzx());
8079         assertNull(calibrator.getEstimatedMzy());
8080         assertNull(calibrator.getEstimatedCovariance());
8081         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8082         assertNull(calibrator.getGroundTruthGravityNorm());
8083         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8084         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8085 
8086         // Force IllegalArgumentException
8087         calibrator = null;
8088         try {
8089             calibrator = new KnownPositionAccelerometerCalibrator(
8090                     new Matrix(1, 1), ma);
8091             fail("IllegalArgumentException expected but not thrown");
8092         } catch (final IllegalArgumentException ignore) {
8093         }
8094         try {
8095             calibrator = new KnownPositionAccelerometerCalibrator(
8096                     new Matrix(1, 3), ma);
8097             fail("IllegalArgumentException expected but not thrown");
8098         } catch (final IllegalArgumentException ignore) {
8099         }
8100         try {
8101             calibrator = new KnownPositionAccelerometerCalibrator(ba,
8102                     new Matrix(1, 3));
8103             fail("IllegalArgumentException expected but not thrown");
8104         } catch (final IllegalArgumentException ignore) {
8105         }
8106         try {
8107             calibrator = new KnownPositionAccelerometerCalibrator(ba,
8108                     new Matrix(3, 1));
8109             fail("IllegalArgumentException expected but not thrown");
8110         } catch (final IllegalArgumentException ignore) {
8111         }
8112         assertNull(calibrator);
8113     }
8114 
8115     @Test
8116     public void testConstructor72() throws WrongSizeException {
8117         final Matrix ba = generateBa();
8118         final double biasX = ba.getElementAtIndex(0);
8119         final double biasY = ba.getElementAtIndex(1);
8120         final double biasZ = ba.getElementAtIndex(2);
8121 
8122         final Matrix ma = generateMaCommonAxis();
8123         final double sx = ma.getElementAt(0, 0);
8124         final double sy = ma.getElementAt(1, 1);
8125         final double sz = ma.getElementAt(2, 2);
8126         final double mxy = ma.getElementAt(0, 1);
8127         final double mxz = ma.getElementAt(0, 2);
8128         final double myx = ma.getElementAt(1, 0);
8129         final double myz = ma.getElementAt(1, 2);
8130         final double mzx = ma.getElementAt(2, 0);
8131         final double mzy = ma.getElementAt(2, 1);
8132 
8133         KnownPositionAccelerometerCalibrator calibrator =
8134                 new KnownPositionAccelerometerCalibrator(ba, ma, this);
8135 
8136         // check default values
8137         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8138         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8139         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8140         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8141         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8142         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8143         final Acceleration bx2 = new Acceleration(0.0,
8144                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8145         calibrator.getInitialBiasXAsAcceleration(bx2);
8146         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8147         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8148         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8149         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8150         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8151         final Acceleration by2 = new Acceleration(0.0,
8152                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8153         calibrator.getInitialBiasYAsAcceleration(by2);
8154         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8155         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8156         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8157         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8158         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8159         final Acceleration bz2 = new Acceleration(0.0,
8160                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8161         calibrator.getInitialBiasZAsAcceleration(bz2);
8162         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8163         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8164         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8165         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8166         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8167         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8168         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8169         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8170         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8171         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8172         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8173         final double[] bias1 = calibrator.getInitialBias();
8174         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8175         final double[] bias2 = new double[3];
8176         calibrator.getInitialBias(bias2);
8177         assertArrayEquals(bias1, bias2, 0.0);
8178         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8179         assertEquals(b1, ba);
8180         final Matrix b2 = new Matrix(3, 1);
8181         calibrator.getInitialBiasAsMatrix(b2);
8182         assertEquals(b1, b2);
8183         final Matrix ma1 = new Matrix(3, 3);
8184         ma1.setSubmatrix(0, 0,
8185                 2, 2,
8186                 new double[]{sx, myx, mzx,
8187                         mxy, sy, mzy,
8188                         mxz, myz, sz});
8189         assertEquals(calibrator.getInitialMa(), ma1);
8190         final Matrix ma2 = new Matrix(3, 3);
8191         calibrator.getInitialMa(ma2);
8192         assertEquals(ma1, ma2);
8193         assertNull(calibrator.getEcefPosition());
8194         assertNull(calibrator.getNedPosition());
8195         assertFalse(calibrator.getNedPosition(null));
8196         assertNull(calibrator.getMeasurements());
8197         assertFalse(calibrator.isCommonAxisUsed());
8198         assertSame(calibrator.getListener(), this);
8199         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8200         assertFalse(calibrator.isReady());
8201         assertFalse(calibrator.isRunning());
8202         assertNull(calibrator.getEstimatedBiases());
8203         assertFalse(calibrator.getEstimatedBiases(null));
8204         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8205         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8206         assertNull(calibrator.getEstimatedBiasFx());
8207         assertNull(calibrator.getEstimatedBiasFy());
8208         assertNull(calibrator.getEstimatedBiasFz());
8209         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8210         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8211         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8212         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8213         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8214         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8215         assertNull(calibrator.getEstimatedMa());
8216         assertNull(calibrator.getEstimatedSx());
8217         assertNull(calibrator.getEstimatedSy());
8218         assertNull(calibrator.getEstimatedSz());
8219         assertNull(calibrator.getEstimatedMxy());
8220         assertNull(calibrator.getEstimatedMxz());
8221         assertNull(calibrator.getEstimatedMyx());
8222         assertNull(calibrator.getEstimatedMyz());
8223         assertNull(calibrator.getEstimatedMzx());
8224         assertNull(calibrator.getEstimatedMzy());
8225         assertNull(calibrator.getEstimatedCovariance());
8226         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8227         assertNull(calibrator.getGroundTruthGravityNorm());
8228         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8229         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8230 
8231         // Force IllegalArgumentException
8232         calibrator = null;
8233         try {
8234             calibrator = new KnownPositionAccelerometerCalibrator(
8235                     new Matrix(1, 1), ma, this);
8236             fail("IllegalArgumentException expected but not thrown");
8237         } catch (final IllegalArgumentException ignore) {
8238         }
8239         try {
8240             calibrator = new KnownPositionAccelerometerCalibrator(
8241                     new Matrix(1, 3), ma, this);
8242             fail("IllegalArgumentException expected but not thrown");
8243         } catch (final IllegalArgumentException ignore) {
8244         }
8245         try {
8246             calibrator = new KnownPositionAccelerometerCalibrator(ba,
8247                     new Matrix(1, 3), this);
8248             fail("IllegalArgumentException expected but not thrown");
8249         } catch (final IllegalArgumentException ignore) {
8250         }
8251         try {
8252             calibrator = new KnownPositionAccelerometerCalibrator(ba,
8253                     new Matrix(3, 1), this);
8254             fail("IllegalArgumentException expected but not thrown");
8255         } catch (final IllegalArgumentException ignore) {
8256         }
8257         assertNull(calibrator);
8258     }
8259 
8260     @Test
8261     public void testConstructor73() throws WrongSizeException {
8262         final Collection<StandardDeviationBodyKinematics> measurements =
8263                 Collections.emptyList();
8264 
8265         final Matrix ba = generateBa();
8266         final double biasX = ba.getElementAtIndex(0);
8267         final double biasY = ba.getElementAtIndex(1);
8268         final double biasZ = ba.getElementAtIndex(2);
8269 
8270         final Matrix ma = generateMaCommonAxis();
8271         final double sx = ma.getElementAt(0, 0);
8272         final double sy = ma.getElementAt(1, 1);
8273         final double sz = ma.getElementAt(2, 2);
8274         final double mxy = ma.getElementAt(0, 1);
8275         final double mxz = ma.getElementAt(0, 2);
8276         final double myx = ma.getElementAt(1, 0);
8277         final double myz = ma.getElementAt(1, 2);
8278         final double mzx = ma.getElementAt(2, 0);
8279         final double mzy = ma.getElementAt(2, 1);
8280 
8281         KnownPositionAccelerometerCalibrator calibrator =
8282                 new KnownPositionAccelerometerCalibrator(measurements, ba, ma);
8283 
8284         // check default values
8285         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8286         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8287         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8288         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8289         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8290         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8291         final Acceleration bx2 = new Acceleration(0.0,
8292                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8293         calibrator.getInitialBiasXAsAcceleration(bx2);
8294         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8295         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8296         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8297         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8298         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8299         final Acceleration by2 = new Acceleration(0.0,
8300                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8301         calibrator.getInitialBiasYAsAcceleration(by2);
8302         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8303         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8304         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8305         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8306         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8307         final Acceleration bz2 = new Acceleration(0.0,
8308                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8309         calibrator.getInitialBiasZAsAcceleration(bz2);
8310         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8311         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8312         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8313         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8314         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8315         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8316         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8317         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8318         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8319         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8320         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8321         final double[] bias1 = calibrator.getInitialBias();
8322         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8323         final double[] bias2 = new double[3];
8324         calibrator.getInitialBias(bias2);
8325         assertArrayEquals(bias1, bias2, 0.0);
8326         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8327         assertEquals(b1, ba);
8328         final Matrix b2 = new Matrix(3, 1);
8329         calibrator.getInitialBiasAsMatrix(b2);
8330         assertEquals(b1, b2);
8331         final Matrix ma1 = new Matrix(3, 3);
8332         ma1.setSubmatrix(0, 0,
8333                 2, 2,
8334                 new double[]{sx, myx, mzx,
8335                         mxy, sy, mzy,
8336                         mxz, myz, sz});
8337         assertEquals(calibrator.getInitialMa(), ma1);
8338         final Matrix ma2 = new Matrix(3, 3);
8339         calibrator.getInitialMa(ma2);
8340         assertEquals(ma1, ma2);
8341         assertNull(calibrator.getEcefPosition());
8342         assertNull(calibrator.getNedPosition());
8343         assertFalse(calibrator.getNedPosition(null));
8344         assertSame(calibrator.getMeasurements(), measurements);
8345         assertFalse(calibrator.isCommonAxisUsed());
8346         assertNull(calibrator.getListener());
8347         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8348         assertFalse(calibrator.isReady());
8349         assertFalse(calibrator.isRunning());
8350         assertNull(calibrator.getEstimatedBiases());
8351         assertFalse(calibrator.getEstimatedBiases(null));
8352         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8353         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8354         assertNull(calibrator.getEstimatedBiasFx());
8355         assertNull(calibrator.getEstimatedBiasFy());
8356         assertNull(calibrator.getEstimatedBiasFz());
8357         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8358         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8359         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8360         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8361         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8362         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8363         assertNull(calibrator.getEstimatedMa());
8364         assertNull(calibrator.getEstimatedSx());
8365         assertNull(calibrator.getEstimatedSy());
8366         assertNull(calibrator.getEstimatedSz());
8367         assertNull(calibrator.getEstimatedMxy());
8368         assertNull(calibrator.getEstimatedMxz());
8369         assertNull(calibrator.getEstimatedMyx());
8370         assertNull(calibrator.getEstimatedMyz());
8371         assertNull(calibrator.getEstimatedMzx());
8372         assertNull(calibrator.getEstimatedMzy());
8373         assertNull(calibrator.getEstimatedCovariance());
8374         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8375         assertNull(calibrator.getGroundTruthGravityNorm());
8376         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8377         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8378 
8379         // Force IllegalArgumentException
8380         calibrator = null;
8381         try {
8382             calibrator = new KnownPositionAccelerometerCalibrator(
8383                     measurements, new Matrix(1, 1), ma);
8384             fail("IllegalArgumentException expected but not thrown");
8385         } catch (final IllegalArgumentException ignore) {
8386         }
8387         try {
8388             calibrator = new KnownPositionAccelerometerCalibrator(
8389                     measurements, new Matrix(1, 3), ma);
8390             fail("IllegalArgumentException expected but not thrown");
8391         } catch (final IllegalArgumentException ignore) {
8392         }
8393         try {
8394             calibrator = new KnownPositionAccelerometerCalibrator(
8395                     measurements, ba, new Matrix(1, 3));
8396             fail("IllegalArgumentException expected but not thrown");
8397         } catch (final IllegalArgumentException ignore) {
8398         }
8399         try {
8400             calibrator = new KnownPositionAccelerometerCalibrator(
8401                     measurements, ba, new Matrix(3, 1));
8402             fail("IllegalArgumentException expected but not thrown");
8403         } catch (final IllegalArgumentException ignore) {
8404         }
8405         assertNull(calibrator);
8406     }
8407 
8408     @Test
8409     public void testConstructor74() throws WrongSizeException {
8410         final Collection<StandardDeviationBodyKinematics> measurements =
8411                 Collections.emptyList();
8412 
8413         final Matrix ba = generateBa();
8414         final double biasX = ba.getElementAtIndex(0);
8415         final double biasY = ba.getElementAtIndex(1);
8416         final double biasZ = ba.getElementAtIndex(2);
8417 
8418         final Matrix ma = generateMaCommonAxis();
8419         final double sx = ma.getElementAt(0, 0);
8420         final double sy = ma.getElementAt(1, 1);
8421         final double sz = ma.getElementAt(2, 2);
8422         final double mxy = ma.getElementAt(0, 1);
8423         final double mxz = ma.getElementAt(0, 2);
8424         final double myx = ma.getElementAt(1, 0);
8425         final double myz = ma.getElementAt(1, 2);
8426         final double mzx = ma.getElementAt(2, 0);
8427         final double mzy = ma.getElementAt(2, 1);
8428 
8429         KnownPositionAccelerometerCalibrator calibrator =
8430                 new KnownPositionAccelerometerCalibrator(measurements, ba, ma,
8431                         this);
8432 
8433         // check default values
8434         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8435         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8436         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8437         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8438         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8439         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8440         final Acceleration bx2 = new Acceleration(0.0,
8441                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8442         calibrator.getInitialBiasXAsAcceleration(bx2);
8443         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8444         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8445         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8446         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8447         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8448         final Acceleration by2 = new Acceleration(0.0,
8449                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8450         calibrator.getInitialBiasYAsAcceleration(by2);
8451         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8452         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8453         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8454         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8455         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8456         final Acceleration bz2 = new Acceleration(0.0,
8457                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8458         calibrator.getInitialBiasZAsAcceleration(bz2);
8459         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8460         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8461         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8462         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8463         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8464         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8465         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8466         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8467         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8468         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8469         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8470         final double[] bias1 = calibrator.getInitialBias();
8471         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8472         final double[] bias2 = new double[3];
8473         calibrator.getInitialBias(bias2);
8474         assertArrayEquals(bias1, bias2, 0.0);
8475         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8476         assertEquals(b1, ba);
8477         final Matrix b2 = new Matrix(3, 1);
8478         calibrator.getInitialBiasAsMatrix(b2);
8479         assertEquals(b1, b2);
8480         final Matrix ma1 = new Matrix(3, 3);
8481         ma1.setSubmatrix(0, 0,
8482                 2, 2,
8483                 new double[]{sx, myx, mzx,
8484                         mxy, sy, mzy,
8485                         mxz, myz, sz});
8486         assertEquals(calibrator.getInitialMa(), ma1);
8487         final Matrix ma2 = new Matrix(3, 3);
8488         calibrator.getInitialMa(ma2);
8489         assertEquals(ma1, ma2);
8490         assertNull(calibrator.getEcefPosition());
8491         assertNull(calibrator.getNedPosition());
8492         assertFalse(calibrator.getNedPosition(null));
8493         assertSame(calibrator.getMeasurements(), measurements);
8494         assertFalse(calibrator.isCommonAxisUsed());
8495         assertSame(calibrator.getListener(), this);
8496         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8497         assertFalse(calibrator.isReady());
8498         assertFalse(calibrator.isRunning());
8499         assertNull(calibrator.getEstimatedBiases());
8500         assertFalse(calibrator.getEstimatedBiases(null));
8501         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8502         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8503         assertNull(calibrator.getEstimatedBiasFx());
8504         assertNull(calibrator.getEstimatedBiasFy());
8505         assertNull(calibrator.getEstimatedBiasFz());
8506         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8507         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8508         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8509         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8510         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8511         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8512         assertNull(calibrator.getEstimatedMa());
8513         assertNull(calibrator.getEstimatedSx());
8514         assertNull(calibrator.getEstimatedSy());
8515         assertNull(calibrator.getEstimatedSz());
8516         assertNull(calibrator.getEstimatedMxy());
8517         assertNull(calibrator.getEstimatedMxz());
8518         assertNull(calibrator.getEstimatedMyx());
8519         assertNull(calibrator.getEstimatedMyz());
8520         assertNull(calibrator.getEstimatedMzx());
8521         assertNull(calibrator.getEstimatedMzy());
8522         assertNull(calibrator.getEstimatedCovariance());
8523         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8524         assertNull(calibrator.getGroundTruthGravityNorm());
8525         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8526         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8527 
8528         // Force IllegalArgumentException
8529         calibrator = null;
8530         try {
8531             calibrator = new KnownPositionAccelerometerCalibrator(
8532                     measurements, new Matrix(1, 1), ma, this);
8533             fail("IllegalArgumentException expected but not thrown");
8534         } catch (final IllegalArgumentException ignore) {
8535         }
8536         try {
8537             calibrator = new KnownPositionAccelerometerCalibrator(
8538                     measurements, new Matrix(1, 3), ma, this);
8539             fail("IllegalArgumentException expected but not thrown");
8540         } catch (final IllegalArgumentException ignore) {
8541         }
8542         try {
8543             calibrator = new KnownPositionAccelerometerCalibrator(
8544                     measurements, ba, new Matrix(1, 3), this);
8545             fail("IllegalArgumentException expected but not thrown");
8546         } catch (final IllegalArgumentException ignore) {
8547         }
8548         try {
8549             calibrator = new KnownPositionAccelerometerCalibrator(
8550                     measurements, ba, new Matrix(3, 1), this);
8551             fail("IllegalArgumentException expected but not thrown");
8552         } catch (final IllegalArgumentException ignore) {
8553         }
8554         assertNull(calibrator);
8555     }
8556 
8557     @Test
8558     public void testConstructor75() throws WrongSizeException {
8559         final Matrix ba = generateBa();
8560         final double biasX = ba.getElementAtIndex(0);
8561         final double biasY = ba.getElementAtIndex(1);
8562         final double biasZ = ba.getElementAtIndex(2);
8563 
8564         final Matrix ma = generateMaCommonAxis();
8565         final double sx = ma.getElementAt(0, 0);
8566         final double sy = ma.getElementAt(1, 1);
8567         final double sz = ma.getElementAt(2, 2);
8568         final double mxy = ma.getElementAt(0, 1);
8569         final double mxz = ma.getElementAt(0, 2);
8570         final double myx = ma.getElementAt(1, 0);
8571         final double myz = ma.getElementAt(1, 2);
8572         final double mzx = ma.getElementAt(2, 0);
8573         final double mzy = ma.getElementAt(2, 1);
8574 
8575         KnownPositionAccelerometerCalibrator calibrator =
8576                 new KnownPositionAccelerometerCalibrator(true, ba, ma);
8577 
8578         // check default values
8579         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8580         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8581         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8582         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8583         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8584         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8585         final Acceleration bx2 = new Acceleration(0.0,
8586                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8587         calibrator.getInitialBiasXAsAcceleration(bx2);
8588         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8589         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8590         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8591         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8592         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8593         final Acceleration by2 = new Acceleration(0.0,
8594                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8595         calibrator.getInitialBiasYAsAcceleration(by2);
8596         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8597         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8598         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8599         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8600         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8601         final Acceleration bz2 = new Acceleration(0.0,
8602                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8603         calibrator.getInitialBiasZAsAcceleration(bz2);
8604         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8605         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8606         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8607         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8608         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8609         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8610         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8611         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8612         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8613         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8614         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8615         final double[] bias1 = calibrator.getInitialBias();
8616         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8617         final double[] bias2 = new double[3];
8618         calibrator.getInitialBias(bias2);
8619         assertArrayEquals(bias1, bias2, 0.0);
8620         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8621         assertEquals(b1, ba);
8622         final Matrix b2 = new Matrix(3, 1);
8623         calibrator.getInitialBiasAsMatrix(b2);
8624         assertEquals(b1, b2);
8625         final Matrix ma1 = new Matrix(3, 3);
8626         ma1.setSubmatrix(0, 0,
8627                 2, 2,
8628                 new double[]{sx, myx, mzx,
8629                         mxy, sy, mzy,
8630                         mxz, myz, sz});
8631         assertEquals(calibrator.getInitialMa(), ma1);
8632         final Matrix ma2 = new Matrix(3, 3);
8633         calibrator.getInitialMa(ma2);
8634         assertEquals(ma1, ma2);
8635         assertNull(calibrator.getEcefPosition());
8636         assertNull(calibrator.getNedPosition());
8637         assertFalse(calibrator.getNedPosition(null));
8638         assertNull(calibrator.getMeasurements());
8639         assertTrue(calibrator.isCommonAxisUsed());
8640         assertNull(calibrator.getListener());
8641         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8642         assertFalse(calibrator.isReady());
8643         assertFalse(calibrator.isRunning());
8644         assertNull(calibrator.getEstimatedBiases());
8645         assertFalse(calibrator.getEstimatedBiases(null));
8646         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8647         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8648         assertNull(calibrator.getEstimatedBiasFx());
8649         assertNull(calibrator.getEstimatedBiasFy());
8650         assertNull(calibrator.getEstimatedBiasFz());
8651         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8652         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8653         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8654         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8655         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8656         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8657         assertNull(calibrator.getEstimatedMa());
8658         assertNull(calibrator.getEstimatedSx());
8659         assertNull(calibrator.getEstimatedSy());
8660         assertNull(calibrator.getEstimatedSz());
8661         assertNull(calibrator.getEstimatedMxy());
8662         assertNull(calibrator.getEstimatedMxz());
8663         assertNull(calibrator.getEstimatedMyx());
8664         assertNull(calibrator.getEstimatedMyz());
8665         assertNull(calibrator.getEstimatedMzx());
8666         assertNull(calibrator.getEstimatedMzy());
8667         assertNull(calibrator.getEstimatedCovariance());
8668         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8669         assertNull(calibrator.getGroundTruthGravityNorm());
8670         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8671         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8672 
8673         // Force IllegalArgumentException
8674         calibrator = null;
8675         try {
8676             calibrator = new KnownPositionAccelerometerCalibrator(
8677                     true, new Matrix(1, 1), ma);
8678             fail("IllegalArgumentException expected but not thrown");
8679         } catch (final IllegalArgumentException ignore) {
8680         }
8681         try {
8682             calibrator = new KnownPositionAccelerometerCalibrator(
8683                     true, new Matrix(1, 3), ma);
8684             fail("IllegalArgumentException expected but not thrown");
8685         } catch (final IllegalArgumentException ignore) {
8686         }
8687         try {
8688             calibrator = new KnownPositionAccelerometerCalibrator(
8689                     true, ba, new Matrix(1, 3));
8690             fail("IllegalArgumentException expected but not thrown");
8691         } catch (final IllegalArgumentException ignore) {
8692         }
8693         try {
8694             calibrator = new KnownPositionAccelerometerCalibrator(
8695                     true, ba, new Matrix(3, 1));
8696             fail("IllegalArgumentException expected but not thrown");
8697         } catch (final IllegalArgumentException ignore) {
8698         }
8699         assertNull(calibrator);
8700     }
8701 
8702     @Test
8703     public void testConstructor76() throws WrongSizeException {
8704         final Matrix ba = generateBa();
8705         final double biasX = ba.getElementAtIndex(0);
8706         final double biasY = ba.getElementAtIndex(1);
8707         final double biasZ = ba.getElementAtIndex(2);
8708 
8709         final Matrix ma = generateMaCommonAxis();
8710         final double sx = ma.getElementAt(0, 0);
8711         final double sy = ma.getElementAt(1, 1);
8712         final double sz = ma.getElementAt(2, 2);
8713         final double mxy = ma.getElementAt(0, 1);
8714         final double mxz = ma.getElementAt(0, 2);
8715         final double myx = ma.getElementAt(1, 0);
8716         final double myz = ma.getElementAt(1, 2);
8717         final double mzx = ma.getElementAt(2, 0);
8718         final double mzy = ma.getElementAt(2, 1);
8719 
8720         KnownPositionAccelerometerCalibrator calibrator =
8721                 new KnownPositionAccelerometerCalibrator(true, ba, ma,
8722                         this);
8723 
8724         // check default values
8725         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8726         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8727         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8728         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8729         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8730         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8731         final Acceleration bx2 = new Acceleration(0.0,
8732                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8733         calibrator.getInitialBiasXAsAcceleration(bx2);
8734         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8735         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8736         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8737         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8738         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8739         final Acceleration by2 = new Acceleration(0.0,
8740                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8741         calibrator.getInitialBiasYAsAcceleration(by2);
8742         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8743         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8744         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8745         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8746         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8747         final Acceleration bz2 = new Acceleration(0.0,
8748                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8749         calibrator.getInitialBiasZAsAcceleration(bz2);
8750         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8751         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8752         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8753         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8754         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8755         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8756         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8757         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8758         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8759         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8760         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8761         final double[] bias1 = calibrator.getInitialBias();
8762         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8763         final double[] bias2 = new double[3];
8764         calibrator.getInitialBias(bias2);
8765         assertArrayEquals(bias1, bias2, 0.0);
8766         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8767         assertEquals(b1, ba);
8768         final Matrix b2 = new Matrix(3, 1);
8769         calibrator.getInitialBiasAsMatrix(b2);
8770         assertEquals(b1, b2);
8771         final Matrix ma1 = new Matrix(3, 3);
8772         ma1.setSubmatrix(0, 0,
8773                 2, 2,
8774                 new double[]{sx, myx, mzx,
8775                         mxy, sy, mzy,
8776                         mxz, myz, sz});
8777         assertEquals(calibrator.getInitialMa(), ma1);
8778         final Matrix ma2 = new Matrix(3, 3);
8779         calibrator.getInitialMa(ma2);
8780         assertEquals(ma1, ma2);
8781         assertNull(calibrator.getEcefPosition());
8782         assertNull(calibrator.getNedPosition());
8783         assertFalse(calibrator.getNedPosition(null));
8784         assertNull(calibrator.getMeasurements());
8785         assertTrue(calibrator.isCommonAxisUsed());
8786         assertSame(calibrator.getListener(), this);
8787         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8788         assertFalse(calibrator.isReady());
8789         assertFalse(calibrator.isRunning());
8790         assertNull(calibrator.getEstimatedBiases());
8791         assertFalse(calibrator.getEstimatedBiases(null));
8792         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8793         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8794         assertNull(calibrator.getEstimatedBiasFx());
8795         assertNull(calibrator.getEstimatedBiasFy());
8796         assertNull(calibrator.getEstimatedBiasFz());
8797         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8798         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8799         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8800         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8801         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8802         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8803         assertNull(calibrator.getEstimatedMa());
8804         assertNull(calibrator.getEstimatedSx());
8805         assertNull(calibrator.getEstimatedSy());
8806         assertNull(calibrator.getEstimatedSz());
8807         assertNull(calibrator.getEstimatedMxy());
8808         assertNull(calibrator.getEstimatedMxz());
8809         assertNull(calibrator.getEstimatedMyx());
8810         assertNull(calibrator.getEstimatedMyz());
8811         assertNull(calibrator.getEstimatedMzx());
8812         assertNull(calibrator.getEstimatedMzy());
8813         assertNull(calibrator.getEstimatedCovariance());
8814         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8815         assertNull(calibrator.getGroundTruthGravityNorm());
8816         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8817         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8818 
8819         // Force IllegalArgumentException
8820         calibrator = null;
8821         try {
8822             calibrator = new KnownPositionAccelerometerCalibrator(
8823                     true, new Matrix(1, 1), ma,
8824                     this);
8825             fail("IllegalArgumentException expected but not thrown");
8826         } catch (final IllegalArgumentException ignore) {
8827         }
8828         try {
8829             calibrator = new KnownPositionAccelerometerCalibrator(
8830                     true, new Matrix(1, 3), ma,
8831                     this);
8832             fail("IllegalArgumentException expected but not thrown");
8833         } catch (final IllegalArgumentException ignore) {
8834         }
8835         try {
8836             calibrator = new KnownPositionAccelerometerCalibrator(
8837                     true, ba, new Matrix(1, 3),
8838                     this);
8839             fail("IllegalArgumentException expected but not thrown");
8840         } catch (final IllegalArgumentException ignore) {
8841         }
8842         try {
8843             calibrator = new KnownPositionAccelerometerCalibrator(
8844                     true, ba, new Matrix(3, 1),
8845                     this);
8846             fail("IllegalArgumentException expected but not thrown");
8847         } catch (final IllegalArgumentException ignore) {
8848         }
8849         assertNull(calibrator);
8850     }
8851 
8852     @Test
8853     public void testConstructor77() throws WrongSizeException {
8854         final Collection<StandardDeviationBodyKinematics> measurements =
8855                 Collections.emptyList();
8856 
8857         final Matrix ba = generateBa();
8858         final double biasX = ba.getElementAtIndex(0);
8859         final double biasY = ba.getElementAtIndex(1);
8860         final double biasZ = ba.getElementAtIndex(2);
8861 
8862         final Matrix ma = generateMaCommonAxis();
8863         final double sx = ma.getElementAt(0, 0);
8864         final double sy = ma.getElementAt(1, 1);
8865         final double sz = ma.getElementAt(2, 2);
8866         final double mxy = ma.getElementAt(0, 1);
8867         final double mxz = ma.getElementAt(0, 2);
8868         final double myx = ma.getElementAt(1, 0);
8869         final double myz = ma.getElementAt(1, 2);
8870         final double mzx = ma.getElementAt(2, 0);
8871         final double mzy = ma.getElementAt(2, 1);
8872 
8873         KnownPositionAccelerometerCalibrator calibrator =
8874                 new KnownPositionAccelerometerCalibrator(measurements,
8875                         true, ba, ma);
8876 
8877         // check default values
8878         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8879         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8880         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8881         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8882         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8883         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8884         final Acceleration bx2 = new Acceleration(0.0,
8885                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8886         calibrator.getInitialBiasXAsAcceleration(bx2);
8887         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8888         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8889         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8890         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8891         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8892         final Acceleration by2 = new Acceleration(0.0,
8893                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8894         calibrator.getInitialBiasYAsAcceleration(by2);
8895         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8896         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8897         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8898         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8899         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8900         final Acceleration bz2 = new Acceleration(0.0,
8901                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8902         calibrator.getInitialBiasZAsAcceleration(bz2);
8903         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8904         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8905         assertEquals(calibrator.getInitialSx(), sx, 0.0);
8906         assertEquals(calibrator.getInitialSy(), sy, 0.0);
8907         assertEquals(calibrator.getInitialSz(), sz, 0.0);
8908         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8909         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8910         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8911         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8912         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8913         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8914         final double[] bias1 = calibrator.getInitialBias();
8915         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8916         final double[] bias2 = new double[3];
8917         calibrator.getInitialBias(bias2);
8918         assertArrayEquals(bias1, bias2, 0.0);
8919         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8920         assertEquals(b1, ba);
8921         final Matrix b2 = new Matrix(3, 1);
8922         calibrator.getInitialBiasAsMatrix(b2);
8923         assertEquals(b1, b2);
8924         final Matrix ma1 = new Matrix(3, 3);
8925         ma1.setSubmatrix(0, 0,
8926                 2, 2,
8927                 new double[]{sx, myx, mzx,
8928                         mxy, sy, mzy,
8929                         mxz, myz, sz});
8930         assertEquals(calibrator.getInitialMa(), ma1);
8931         final Matrix ma2 = new Matrix(3, 3);
8932         calibrator.getInitialMa(ma2);
8933         assertEquals(ma1, ma2);
8934         assertNull(calibrator.getEcefPosition());
8935         assertNull(calibrator.getNedPosition());
8936         assertFalse(calibrator.getNedPosition(null));
8937         assertSame(calibrator.getMeasurements(), measurements);
8938         assertTrue(calibrator.isCommonAxisUsed());
8939         assertNull(calibrator.getListener());
8940         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8941         assertFalse(calibrator.isReady());
8942         assertFalse(calibrator.isRunning());
8943         assertNull(calibrator.getEstimatedBiases());
8944         assertFalse(calibrator.getEstimatedBiases(null));
8945         assertNull(calibrator.getEstimatedBiasesAsMatrix());
8946         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8947         assertNull(calibrator.getEstimatedBiasFx());
8948         assertNull(calibrator.getEstimatedBiasFy());
8949         assertNull(calibrator.getEstimatedBiasFz());
8950         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8951         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8952         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8953         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8954         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8955         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8956         assertNull(calibrator.getEstimatedMa());
8957         assertNull(calibrator.getEstimatedSx());
8958         assertNull(calibrator.getEstimatedSy());
8959         assertNull(calibrator.getEstimatedSz());
8960         assertNull(calibrator.getEstimatedMxy());
8961         assertNull(calibrator.getEstimatedMxz());
8962         assertNull(calibrator.getEstimatedMyx());
8963         assertNull(calibrator.getEstimatedMyz());
8964         assertNull(calibrator.getEstimatedMzx());
8965         assertNull(calibrator.getEstimatedMzy());
8966         assertNull(calibrator.getEstimatedCovariance());
8967         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8968         assertNull(calibrator.getGroundTruthGravityNorm());
8969         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8970         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8971 
8972         // Force IllegalArgumentException
8973         calibrator = null;
8974         try {
8975             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8976                     true, new Matrix(1, 1), ma);
8977             fail("IllegalArgumentException expected but not thrown");
8978         } catch (final IllegalArgumentException ignore) {
8979         }
8980         try {
8981             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8982                     true, new Matrix(1, 3), ma);
8983             fail("IllegalArgumentException expected but not thrown");
8984         } catch (final IllegalArgumentException ignore) {
8985         }
8986         try {
8987             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8988                     true, ba, new Matrix(1, 3));
8989             fail("IllegalArgumentException expected but not thrown");
8990         } catch (final IllegalArgumentException ignore) {
8991         }
8992         try {
8993             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8994                     true, ba, new Matrix(3, 1));
8995             fail("IllegalArgumentException expected but not thrown");
8996         } catch (final IllegalArgumentException ignore) {
8997         }
8998         assertNull(calibrator);
8999     }
9000 
9001     @Test
9002     public void testConstructor78() throws WrongSizeException {
9003         final Collection<StandardDeviationBodyKinematics> measurements =
9004                 Collections.emptyList();
9005 
9006         final Matrix ba = generateBa();
9007         final double biasX = ba.getElementAtIndex(0);
9008         final double biasY = ba.getElementAtIndex(1);
9009         final double biasZ = ba.getElementAtIndex(2);
9010 
9011         final Matrix ma = generateMaCommonAxis();
9012         final double sx = ma.getElementAt(0, 0);
9013         final double sy = ma.getElementAt(1, 1);
9014         final double sz = ma.getElementAt(2, 2);
9015         final double mxy = ma.getElementAt(0, 1);
9016         final double mxz = ma.getElementAt(0, 2);
9017         final double myx = ma.getElementAt(1, 0);
9018         final double myz = ma.getElementAt(1, 2);
9019         final double mzx = ma.getElementAt(2, 0);
9020         final double mzy = ma.getElementAt(2, 1);
9021 
9022         KnownPositionAccelerometerCalibrator calibrator =
9023                 new KnownPositionAccelerometerCalibrator(measurements,
9024                         true, ba, ma, this);
9025 
9026         // check default values
9027         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
9028         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
9029         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
9030         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9031         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9032         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9033         final Acceleration bx2 = new Acceleration(0.0,
9034                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9035         calibrator.getInitialBiasXAsAcceleration(bx2);
9036         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9037         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9038         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9039         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9040         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9041         final Acceleration by2 = new Acceleration(0.0,
9042                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9043         calibrator.getInitialBiasYAsAcceleration(by2);
9044         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9045         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9046         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9047         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9048         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9049         final Acceleration bz2 = new Acceleration(0.0,
9050                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9051         calibrator.getInitialBiasZAsAcceleration(bz2);
9052         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9053         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9054         assertEquals(calibrator.getInitialSx(), sx, 0.0);
9055         assertEquals(calibrator.getInitialSy(), sy, 0.0);
9056         assertEquals(calibrator.getInitialSz(), sz, 0.0);
9057         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
9058         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
9059         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
9060         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
9061         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
9062         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
9063         final double[] bias1 = calibrator.getInitialBias();
9064         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9065         final double[] bias2 = new double[3];
9066         calibrator.getInitialBias(bias2);
9067         assertArrayEquals(bias1, bias2, 0.0);
9068         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9069         assertEquals(b1, ba);
9070         final Matrix b2 = new Matrix(3, 1);
9071         calibrator.getInitialBiasAsMatrix(b2);
9072         assertEquals(b1, b2);
9073         final Matrix ma1 = new Matrix(3, 3);
9074         ma1.setSubmatrix(0, 0,
9075                 2, 2,
9076                 new double[]{sx, myx, mzx,
9077                         mxy, sy, mzy,
9078                         mxz, myz, sz});
9079         assertEquals(calibrator.getInitialMa(), ma1);
9080         final Matrix ma2 = new Matrix(3, 3);
9081         calibrator.getInitialMa(ma2);
9082         assertEquals(ma1, ma2);
9083         assertNull(calibrator.getEcefPosition());
9084         assertNull(calibrator.getNedPosition());
9085         assertFalse(calibrator.getNedPosition(null));
9086         assertSame(calibrator.getMeasurements(), measurements);
9087         assertTrue(calibrator.isCommonAxisUsed());
9088         assertSame(calibrator.getListener(), this);
9089         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9090         assertFalse(calibrator.isReady());
9091         assertFalse(calibrator.isRunning());
9092         assertNull(calibrator.getEstimatedBiases());
9093         assertFalse(calibrator.getEstimatedBiases(null));
9094         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9095         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9096         assertNull(calibrator.getEstimatedBiasFx());
9097         assertNull(calibrator.getEstimatedBiasFy());
9098         assertNull(calibrator.getEstimatedBiasFz());
9099         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9100         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9101         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9102         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9103         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9104         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9105         assertNull(calibrator.getEstimatedMa());
9106         assertNull(calibrator.getEstimatedSx());
9107         assertNull(calibrator.getEstimatedSy());
9108         assertNull(calibrator.getEstimatedSz());
9109         assertNull(calibrator.getEstimatedMxy());
9110         assertNull(calibrator.getEstimatedMxz());
9111         assertNull(calibrator.getEstimatedMyx());
9112         assertNull(calibrator.getEstimatedMyz());
9113         assertNull(calibrator.getEstimatedMzx());
9114         assertNull(calibrator.getEstimatedMzy());
9115         assertNull(calibrator.getEstimatedCovariance());
9116         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9117         assertNull(calibrator.getGroundTruthGravityNorm());
9118         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9119         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
9120 
9121         // Force IllegalArgumentException
9122         calibrator = null;
9123         try {
9124             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9125                     true, new Matrix(1, 1), ma,
9126                     this);
9127             fail("IllegalArgumentException expected but not thrown");
9128         } catch (final IllegalArgumentException ignore) {
9129         }
9130         try {
9131             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9132                     true, new Matrix(1, 3), ma,
9133                     this);
9134             fail("IllegalArgumentException expected but not thrown");
9135         } catch (final IllegalArgumentException ignore) {
9136         }
9137         try {
9138             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9139                     true, ba, new Matrix(1, 3),
9140                     this);
9141             fail("IllegalArgumentException expected but not thrown");
9142         } catch (final IllegalArgumentException ignore) {
9143         }
9144         try {
9145             calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9146                     true, ba, new Matrix(3, 1),
9147                     this);
9148             fail("IllegalArgumentException expected but not thrown");
9149         } catch (final IllegalArgumentException ignore) {
9150         }
9151         assertNull(calibrator);
9152     }
9153 
9154     @Test
9155     public void testConstructor79() throws WrongSizeException {
9156         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9157         final double latitude = Math.toRadians(
9158                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9159         final double longitude = Math.toRadians(
9160                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9161         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9162         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9163         final NEDVelocity nedVelocity = new NEDVelocity();
9164         final ECEFPosition ecefPosition = new ECEFPosition();
9165         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9166         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9167                 ecefPosition, ecefVelocity);
9168 
9169         final KnownPositionAccelerometerCalibrator calibrator =
9170                 new KnownPositionAccelerometerCalibrator(ecefPosition);
9171 
9172         // check default values
9173         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9174         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9175         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9176         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9177         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9178         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9179         final Acceleration bx2 = new Acceleration(0.0,
9180                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9181         calibrator.getInitialBiasXAsAcceleration(bx2);
9182         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9183         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9184         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9185         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9186         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9187         final Acceleration by2 = new Acceleration(0.0,
9188                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9189         calibrator.getInitialBiasYAsAcceleration(by2);
9190         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9191         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9192         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9193         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9194         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9195         final Acceleration bz2 = new Acceleration(0.0,
9196                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9197         calibrator.getInitialBiasZAsAcceleration(bz2);
9198         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9199         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9200         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9201         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9202         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9203         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9204         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9205         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9206         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9207         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9208         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9209         final double[] bias1 = calibrator.getInitialBias();
9210         assertArrayEquals(bias1, new double[3], 0.0);
9211         final double[] bias2 = new double[3];
9212         calibrator.getInitialBias(bias2);
9213         assertArrayEquals(bias1, bias2, 0.0);
9214         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9215         assertEquals(b1, new Matrix(3, 1));
9216         final Matrix b2 = new Matrix(3, 1);
9217         calibrator.getInitialBiasAsMatrix(b2);
9218         assertEquals(b1, b2);
9219         final Matrix ma1 = calibrator.getInitialMa();
9220         assertEquals(ma1, new Matrix(3, 3));
9221         final Matrix ma2 = new Matrix(3, 3);
9222         calibrator.getInitialMa(ma2);
9223         assertEquals(ma1, ma2);
9224         assertSame(calibrator.getEcefPosition(), ecefPosition);
9225         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9226         final NEDPosition nedPosition1 = new NEDPosition();
9227         assertTrue(calibrator.getNedPosition(nedPosition1));
9228         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9229         assertNull(calibrator.getMeasurements());
9230         assertFalse(calibrator.isCommonAxisUsed());
9231         assertNull(calibrator.getListener());
9232         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9233         assertFalse(calibrator.isReady());
9234         assertFalse(calibrator.isRunning());
9235         assertNull(calibrator.getEstimatedBiases());
9236         assertFalse(calibrator.getEstimatedBiases(null));
9237         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9238         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9239         assertNull(calibrator.getEstimatedBiasFx());
9240         assertNull(calibrator.getEstimatedBiasFy());
9241         assertNull(calibrator.getEstimatedBiasFz());
9242         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9243         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9244         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9245         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9246         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9247         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9248         assertNull(calibrator.getEstimatedMa());
9249         assertNull(calibrator.getEstimatedSx());
9250         assertNull(calibrator.getEstimatedSy());
9251         assertNull(calibrator.getEstimatedSz());
9252         assertNull(calibrator.getEstimatedMxy());
9253         assertNull(calibrator.getEstimatedMxz());
9254         assertNull(calibrator.getEstimatedMyx());
9255         assertNull(calibrator.getEstimatedMyz());
9256         assertNull(calibrator.getEstimatedMzx());
9257         assertNull(calibrator.getEstimatedMzy());
9258         assertNull(calibrator.getEstimatedCovariance());
9259         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9260         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9261                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9262         assertNotNull(calibrator.getGroundTruthGravityNorm());
9263         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9264         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9265         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9266                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9267         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9268         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9269         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9270     }
9271 
9272     @Test
9273     public void testConstructor80() throws WrongSizeException {
9274         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9275         final double latitude = Math.toRadians(
9276                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9277         final double longitude = Math.toRadians(
9278                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9279         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9280         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9281         final NEDVelocity nedVelocity = new NEDVelocity();
9282         final ECEFPosition ecefPosition = new ECEFPosition();
9283         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9284         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9285                 ecefPosition, ecefVelocity);
9286 
9287         final KnownPositionAccelerometerCalibrator calibrator =
9288                 new KnownPositionAccelerometerCalibrator(ecefPosition, this);
9289 
9290         // check default values
9291         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9292         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9293         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9294         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9295         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9296         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9297         final Acceleration bx2 = new Acceleration(0.0,
9298                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9299         calibrator.getInitialBiasXAsAcceleration(bx2);
9300         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9301         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9302         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9303         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9304         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9305         final Acceleration by2 = new Acceleration(0.0,
9306                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9307         calibrator.getInitialBiasYAsAcceleration(by2);
9308         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9309         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9310         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9311         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9312         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9313         final Acceleration bz2 = new Acceleration(0.0,
9314                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9315         calibrator.getInitialBiasZAsAcceleration(bz2);
9316         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9317         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9318         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9319         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9320         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9321         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9322         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9323         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9324         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9325         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9326         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9327         final double[] bias1 = calibrator.getInitialBias();
9328         assertArrayEquals(bias1, new double[3], 0.0);
9329         final double[] bias2 = new double[3];
9330         calibrator.getInitialBias(bias2);
9331         assertArrayEquals(bias1, bias2, 0.0);
9332         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9333         assertEquals(b1, new Matrix(3, 1));
9334         final Matrix b2 = new Matrix(3, 1);
9335         calibrator.getInitialBiasAsMatrix(b2);
9336         assertEquals(b1, b2);
9337         final Matrix ma1 = calibrator.getInitialMa();
9338         assertEquals(ma1, new Matrix(3, 3));
9339         final Matrix ma2 = new Matrix(3, 3);
9340         calibrator.getInitialMa(ma2);
9341         assertEquals(ma1, ma2);
9342         assertSame(calibrator.getEcefPosition(), ecefPosition);
9343         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9344         final NEDPosition nedPosition1 = new NEDPosition();
9345         assertTrue(calibrator.getNedPosition(nedPosition1));
9346         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9347         assertNull(calibrator.getMeasurements());
9348         assertFalse(calibrator.isCommonAxisUsed());
9349         assertSame(calibrator.getListener(), this);
9350         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9351         assertFalse(calibrator.isReady());
9352         assertFalse(calibrator.isRunning());
9353         assertNull(calibrator.getEstimatedBiases());
9354         assertFalse(calibrator.getEstimatedBiases(null));
9355         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9356         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9357         assertNull(calibrator.getEstimatedBiasFx());
9358         assertNull(calibrator.getEstimatedBiasFy());
9359         assertNull(calibrator.getEstimatedBiasFz());
9360         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9361         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9362         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9363         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9364         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9365         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9366         assertNull(calibrator.getEstimatedMa());
9367         assertNull(calibrator.getEstimatedSx());
9368         assertNull(calibrator.getEstimatedSy());
9369         assertNull(calibrator.getEstimatedSz());
9370         assertNull(calibrator.getEstimatedMxy());
9371         assertNull(calibrator.getEstimatedMxz());
9372         assertNull(calibrator.getEstimatedMyx());
9373         assertNull(calibrator.getEstimatedMyz());
9374         assertNull(calibrator.getEstimatedMzx());
9375         assertNull(calibrator.getEstimatedMzy());
9376         assertNull(calibrator.getEstimatedCovariance());
9377         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9378         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9379                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9380         assertNotNull(calibrator.getGroundTruthGravityNorm());
9381         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9382         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9383         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9384                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9385         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9386         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9387         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9388     }
9389 
9390     @Test
9391     public void testConstructor81() throws WrongSizeException {
9392         final Collection<StandardDeviationBodyKinematics> measurements =
9393                 Collections.emptyList();
9394 
9395         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9396         final double latitude = Math.toRadians(
9397                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9398         final double longitude = Math.toRadians(
9399                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9400         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9401         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9402         final NEDVelocity nedVelocity = new NEDVelocity();
9403         final ECEFPosition ecefPosition = new ECEFPosition();
9404         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9405         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9406                 ecefPosition, ecefVelocity);
9407 
9408         final KnownPositionAccelerometerCalibrator calibrator =
9409                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements);
9410 
9411         // check default values
9412         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9413         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9414         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9415         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9416         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9417         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9418         final Acceleration bx2 = new Acceleration(0.0,
9419                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9420         calibrator.getInitialBiasXAsAcceleration(bx2);
9421         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9422         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9423         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9424         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9425         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9426         final Acceleration by2 = new Acceleration(0.0,
9427                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9428         calibrator.getInitialBiasYAsAcceleration(by2);
9429         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9430         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9431         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9432         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9433         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9434         final Acceleration bz2 = new Acceleration(0.0,
9435                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9436         calibrator.getInitialBiasZAsAcceleration(bz2);
9437         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9438         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9439         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9440         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9441         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9442         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9443         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9444         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9445         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9446         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9447         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9448         final double[] bias1 = calibrator.getInitialBias();
9449         assertArrayEquals(bias1, new double[3], 0.0);
9450         final double[] bias2 = new double[3];
9451         calibrator.getInitialBias(bias2);
9452         assertArrayEquals(bias1, bias2, 0.0);
9453         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9454         assertEquals(b1, new Matrix(3, 1));
9455         final Matrix b2 = new Matrix(3, 1);
9456         calibrator.getInitialBiasAsMatrix(b2);
9457         assertEquals(b1, b2);
9458         final Matrix ma1 = calibrator.getInitialMa();
9459         assertEquals(ma1, new Matrix(3, 3));
9460         final Matrix ma2 = new Matrix(3, 3);
9461         calibrator.getInitialMa(ma2);
9462         assertEquals(ma1, ma2);
9463         assertSame(calibrator.getEcefPosition(), ecefPosition);
9464         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9465         final NEDPosition nedPosition1 = new NEDPosition();
9466         assertTrue(calibrator.getNedPosition(nedPosition1));
9467         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9468         assertSame(calibrator.getMeasurements(), measurements);
9469         assertFalse(calibrator.isCommonAxisUsed());
9470         assertNull(calibrator.getListener());
9471         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9472         assertFalse(calibrator.isReady());
9473         assertFalse(calibrator.isRunning());
9474         assertNull(calibrator.getEstimatedBiases());
9475         assertFalse(calibrator.getEstimatedBiases(null));
9476         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9477         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9478         assertNull(calibrator.getEstimatedBiasFx());
9479         assertNull(calibrator.getEstimatedBiasFy());
9480         assertNull(calibrator.getEstimatedBiasFz());
9481         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9482         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9483         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9484         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9485         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9486         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9487         assertNull(calibrator.getEstimatedMa());
9488         assertNull(calibrator.getEstimatedSx());
9489         assertNull(calibrator.getEstimatedSy());
9490         assertNull(calibrator.getEstimatedSz());
9491         assertNull(calibrator.getEstimatedMxy());
9492         assertNull(calibrator.getEstimatedMxz());
9493         assertNull(calibrator.getEstimatedMyx());
9494         assertNull(calibrator.getEstimatedMyz());
9495         assertNull(calibrator.getEstimatedMzx());
9496         assertNull(calibrator.getEstimatedMzy());
9497         assertNull(calibrator.getEstimatedCovariance());
9498         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9499         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9500                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9501         assertNotNull(calibrator.getGroundTruthGravityNorm());
9502         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9503         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9504         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9505                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9506         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9507         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9508         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9509     }
9510 
9511     @Test
9512     public void testConstructor82() throws WrongSizeException {
9513         final Collection<StandardDeviationBodyKinematics> measurements =
9514                 Collections.emptyList();
9515 
9516         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9517         final double latitude = Math.toRadians(
9518                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9519         final double longitude = Math.toRadians(
9520                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9521         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9522         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9523         final NEDVelocity nedVelocity = new NEDVelocity();
9524         final ECEFPosition ecefPosition = new ECEFPosition();
9525         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9526         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9527                 ecefPosition, ecefVelocity);
9528 
9529         final KnownPositionAccelerometerCalibrator calibrator =
9530                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9531                         this);
9532 
9533         // check default values
9534         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9535         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9536         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9537         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9538         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9539         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9540         final Acceleration bx2 = new Acceleration(0.0,
9541                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9542         calibrator.getInitialBiasXAsAcceleration(bx2);
9543         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9544         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9545         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9546         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9547         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9548         final Acceleration by2 = new Acceleration(0.0,
9549                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9550         calibrator.getInitialBiasYAsAcceleration(by2);
9551         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9552         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9553         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9554         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9555         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9556         final Acceleration bz2 = new Acceleration(0.0,
9557                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9558         calibrator.getInitialBiasZAsAcceleration(bz2);
9559         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9560         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9561         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9562         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9563         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9564         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9565         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9566         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9567         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9568         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9569         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9570         final double[] bias1 = calibrator.getInitialBias();
9571         assertArrayEquals(bias1, new double[3], 0.0);
9572         final double[] bias2 = new double[3];
9573         calibrator.getInitialBias(bias2);
9574         assertArrayEquals(bias1, bias2, 0.0);
9575         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9576         assertEquals(b1, new Matrix(3, 1));
9577         final Matrix b2 = new Matrix(3, 1);
9578         calibrator.getInitialBiasAsMatrix(b2);
9579         assertEquals(b1, b2);
9580         final Matrix ma1 = calibrator.getInitialMa();
9581         assertEquals(ma1, new Matrix(3, 3));
9582         final Matrix ma2 = new Matrix(3, 3);
9583         calibrator.getInitialMa(ma2);
9584         assertEquals(ma1, ma2);
9585         assertSame(calibrator.getEcefPosition(), ecefPosition);
9586         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9587         final NEDPosition nedPosition1 = new NEDPosition();
9588         assertTrue(calibrator.getNedPosition(nedPosition1));
9589         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9590         assertSame(calibrator.getMeasurements(), measurements);
9591         assertFalse(calibrator.isCommonAxisUsed());
9592         assertSame(calibrator.getListener(), this);
9593         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9594         assertFalse(calibrator.isReady());
9595         assertFalse(calibrator.isRunning());
9596         assertNull(calibrator.getEstimatedBiases());
9597         assertFalse(calibrator.getEstimatedBiases(null));
9598         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9599         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9600         assertNull(calibrator.getEstimatedBiasFx());
9601         assertNull(calibrator.getEstimatedBiasFy());
9602         assertNull(calibrator.getEstimatedBiasFz());
9603         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9604         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9605         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9606         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9607         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9608         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9609         assertNull(calibrator.getEstimatedMa());
9610         assertNull(calibrator.getEstimatedSx());
9611         assertNull(calibrator.getEstimatedSy());
9612         assertNull(calibrator.getEstimatedSz());
9613         assertNull(calibrator.getEstimatedMxy());
9614         assertNull(calibrator.getEstimatedMxz());
9615         assertNull(calibrator.getEstimatedMyx());
9616         assertNull(calibrator.getEstimatedMyz());
9617         assertNull(calibrator.getEstimatedMzx());
9618         assertNull(calibrator.getEstimatedMzy());
9619         assertNull(calibrator.getEstimatedCovariance());
9620         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9621         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9622                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9623         assertNotNull(calibrator.getGroundTruthGravityNorm());
9624         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9625         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9626         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9627                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9628         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9629         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9630         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9631     }
9632 
9633     @Test
9634     public void testConstructor83() throws WrongSizeException {
9635         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9636         final double latitude = Math.toRadians(
9637                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9638         final double longitude = Math.toRadians(
9639                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9640         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9641         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9642         final NEDVelocity nedVelocity = new NEDVelocity();
9643         final ECEFPosition ecefPosition = new ECEFPosition();
9644         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9645         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9646                 ecefPosition, ecefVelocity);
9647 
9648         final KnownPositionAccelerometerCalibrator calibrator =
9649                 new KnownPositionAccelerometerCalibrator(ecefPosition,
9650                         true);
9651 
9652         // check default values
9653         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9654         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9655         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9656         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9657         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9658         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9659         final Acceleration bx2 = new Acceleration(0.0,
9660                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9661         calibrator.getInitialBiasXAsAcceleration(bx2);
9662         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9663         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9664         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9665         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9666         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9667         final Acceleration by2 = new Acceleration(0.0,
9668                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9669         calibrator.getInitialBiasYAsAcceleration(by2);
9670         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9671         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9672         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9673         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9674         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9675         final Acceleration bz2 = new Acceleration(0.0,
9676                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9677         calibrator.getInitialBiasZAsAcceleration(bz2);
9678         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9679         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9680         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9681         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9682         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9683         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9684         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9685         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9686         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9687         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9688         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9689         final double[] bias1 = calibrator.getInitialBias();
9690         assertArrayEquals(bias1, new double[3], 0.0);
9691         final double[] bias2 = new double[3];
9692         calibrator.getInitialBias(bias2);
9693         assertArrayEquals(bias1, bias2, 0.0);
9694         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9695         assertEquals(b1, new Matrix(3, 1));
9696         final Matrix b2 = new Matrix(3, 1);
9697         calibrator.getInitialBiasAsMatrix(b2);
9698         assertEquals(b1, b2);
9699         final Matrix ma1 = calibrator.getInitialMa();
9700         assertEquals(ma1, new Matrix(3, 3));
9701         final Matrix ma2 = new Matrix(3, 3);
9702         calibrator.getInitialMa(ma2);
9703         assertEquals(ma1, ma2);
9704         assertSame(calibrator.getEcefPosition(), ecefPosition);
9705         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9706         final NEDPosition nedPosition1 = new NEDPosition();
9707         assertTrue(calibrator.getNedPosition(nedPosition1));
9708         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9709         assertNull(calibrator.getMeasurements());
9710         assertTrue(calibrator.isCommonAxisUsed());
9711         assertNull(calibrator.getListener());
9712         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9713         assertFalse(calibrator.isReady());
9714         assertFalse(calibrator.isRunning());
9715         assertNull(calibrator.getEstimatedBiases());
9716         assertFalse(calibrator.getEstimatedBiases(null));
9717         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9718         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9719         assertNull(calibrator.getEstimatedBiasFx());
9720         assertNull(calibrator.getEstimatedBiasFy());
9721         assertNull(calibrator.getEstimatedBiasFz());
9722         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9723         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9724         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9725         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9726         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9727         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9728         assertNull(calibrator.getEstimatedMa());
9729         assertNull(calibrator.getEstimatedSx());
9730         assertNull(calibrator.getEstimatedSy());
9731         assertNull(calibrator.getEstimatedSz());
9732         assertNull(calibrator.getEstimatedMxy());
9733         assertNull(calibrator.getEstimatedMxz());
9734         assertNull(calibrator.getEstimatedMyx());
9735         assertNull(calibrator.getEstimatedMyz());
9736         assertNull(calibrator.getEstimatedMzx());
9737         assertNull(calibrator.getEstimatedMzy());
9738         assertNull(calibrator.getEstimatedCovariance());
9739         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9740         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9741                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9742         assertNotNull(calibrator.getGroundTruthGravityNorm());
9743         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9744         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9745         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9746                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9747         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9748         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9749         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9750     }
9751 
9752     @Test
9753     public void testConstructor84() throws WrongSizeException {
9754         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9755         final double latitude = Math.toRadians(
9756                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9757         final double longitude = Math.toRadians(
9758                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9759         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9760         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9761         final NEDVelocity nedVelocity = new NEDVelocity();
9762         final ECEFPosition ecefPosition = new ECEFPosition();
9763         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9764         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9765                 ecefPosition, ecefVelocity);
9766 
9767         final KnownPositionAccelerometerCalibrator calibrator =
9768                 new KnownPositionAccelerometerCalibrator(ecefPosition,
9769                         true, this);
9770 
9771         // check default values
9772         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9773         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9774         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9775         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9776         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9777         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9778         final Acceleration bx2 = new Acceleration(0.0,
9779                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9780         calibrator.getInitialBiasXAsAcceleration(bx2);
9781         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9782         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9783         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9784         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9785         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9786         final Acceleration by2 = new Acceleration(0.0,
9787                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9788         calibrator.getInitialBiasYAsAcceleration(by2);
9789         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9790         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9791         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9792         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9793         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9794         final Acceleration bz2 = new Acceleration(0.0,
9795                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9796         calibrator.getInitialBiasZAsAcceleration(bz2);
9797         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9798         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9799         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9800         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9801         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9802         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9803         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9804         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9805         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9806         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9807         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9808         final double[] bias1 = calibrator.getInitialBias();
9809         assertArrayEquals(bias1, new double[3], 0.0);
9810         final double[] bias2 = new double[3];
9811         calibrator.getInitialBias(bias2);
9812         assertArrayEquals(bias1, bias2, 0.0);
9813         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9814         assertEquals(b1, new Matrix(3, 1));
9815         final Matrix b2 = new Matrix(3, 1);
9816         calibrator.getInitialBiasAsMatrix(b2);
9817         assertEquals(b1, b2);
9818         final Matrix ma1 = calibrator.getInitialMa();
9819         assertEquals(ma1, new Matrix(3, 3));
9820         final Matrix ma2 = new Matrix(3, 3);
9821         calibrator.getInitialMa(ma2);
9822         assertEquals(ma1, ma2);
9823         assertSame(calibrator.getEcefPosition(), ecefPosition);
9824         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9825         final NEDPosition nedPosition1 = new NEDPosition();
9826         assertTrue(calibrator.getNedPosition(nedPosition1));
9827         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9828         assertNull(calibrator.getMeasurements());
9829         assertTrue(calibrator.isCommonAxisUsed());
9830         assertSame(calibrator.getListener(), this);
9831         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9832         assertFalse(calibrator.isReady());
9833         assertFalse(calibrator.isRunning());
9834         assertNull(calibrator.getEstimatedBiases());
9835         assertFalse(calibrator.getEstimatedBiases(null));
9836         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9837         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9838         assertNull(calibrator.getEstimatedBiasFx());
9839         assertNull(calibrator.getEstimatedBiasFy());
9840         assertNull(calibrator.getEstimatedBiasFz());
9841         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9842         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9843         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9844         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9845         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9846         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9847         assertNull(calibrator.getEstimatedMa());
9848         assertNull(calibrator.getEstimatedSx());
9849         assertNull(calibrator.getEstimatedSy());
9850         assertNull(calibrator.getEstimatedSz());
9851         assertNull(calibrator.getEstimatedMxy());
9852         assertNull(calibrator.getEstimatedMxz());
9853         assertNull(calibrator.getEstimatedMyx());
9854         assertNull(calibrator.getEstimatedMyz());
9855         assertNull(calibrator.getEstimatedMzx());
9856         assertNull(calibrator.getEstimatedMzy());
9857         assertNull(calibrator.getEstimatedCovariance());
9858         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9859         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9860                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9861         assertNotNull(calibrator.getGroundTruthGravityNorm());
9862         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9863         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9864         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9865                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9866         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9867         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9868         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9869     }
9870 
9871     @Test
9872     public void testConstructor85() throws WrongSizeException {
9873         final Collection<StandardDeviationBodyKinematics> measurements =
9874                 Collections.emptyList();
9875         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9876         final double latitude = Math.toRadians(
9877                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9878         final double longitude = Math.toRadians(
9879                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9880         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9881         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9882         final NEDVelocity nedVelocity = new NEDVelocity();
9883         final ECEFPosition ecefPosition = new ECEFPosition();
9884         final ECEFVelocity ecefVelocity = new ECEFVelocity();
9885         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9886                 ecefPosition, ecefVelocity);
9887 
9888         final KnownPositionAccelerometerCalibrator calibrator =
9889                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9890                         true);
9891 
9892         // check default values
9893         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9894         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9895         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9896         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9897         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9898         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9899         final Acceleration bx2 = new Acceleration(0.0,
9900                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9901         calibrator.getInitialBiasXAsAcceleration(bx2);
9902         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9903         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9904         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9905         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9906         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9907         final Acceleration by2 = new Acceleration(0.0,
9908                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9909         calibrator.getInitialBiasYAsAcceleration(by2);
9910         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9911         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9912         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9913         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9914         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9915         final Acceleration bz2 = new Acceleration(0.0,
9916                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9917         calibrator.getInitialBiasZAsAcceleration(bz2);
9918         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9919         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9920         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9921         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9922         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9923         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9924         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9925         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9926         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9927         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9928         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9929         final double[] bias1 = calibrator.getInitialBias();
9930         assertArrayEquals(bias1, new double[3], 0.0);
9931         final double[] bias2 = new double[3];
9932         calibrator.getInitialBias(bias2);
9933         assertArrayEquals(bias1, bias2, 0.0);
9934         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9935         assertEquals(b1, new Matrix(3, 1));
9936         final Matrix b2 = new Matrix(3, 1);
9937         calibrator.getInitialBiasAsMatrix(b2);
9938         assertEquals(b1, b2);
9939         final Matrix ma1 = calibrator.getInitialMa();
9940         assertEquals(ma1, new Matrix(3, 3));
9941         final Matrix ma2 = new Matrix(3, 3);
9942         calibrator.getInitialMa(ma2);
9943         assertEquals(ma1, ma2);
9944         assertSame(calibrator.getEcefPosition(), ecefPosition);
9945         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9946         final NEDPosition nedPosition1 = new NEDPosition();
9947         assertTrue(calibrator.getNedPosition(nedPosition1));
9948         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9949         assertSame(calibrator.getMeasurements(), measurements);
9950         assertTrue(calibrator.isCommonAxisUsed());
9951         assertNull(calibrator.getListener());
9952         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9953         assertFalse(calibrator.isReady());
9954         assertFalse(calibrator.isRunning());
9955         assertNull(calibrator.getEstimatedBiases());
9956         assertFalse(calibrator.getEstimatedBiases(null));
9957         assertNull(calibrator.getEstimatedBiasesAsMatrix());
9958         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9959         assertNull(calibrator.getEstimatedBiasFx());
9960         assertNull(calibrator.getEstimatedBiasFy());
9961         assertNull(calibrator.getEstimatedBiasFz());
9962         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9963         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9964         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9965         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9966         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9967         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9968         assertNull(calibrator.getEstimatedMa());
9969         assertNull(calibrator.getEstimatedSx());
9970         assertNull(calibrator.getEstimatedSy());
9971         assertNull(calibrator.getEstimatedSz());
9972         assertNull(calibrator.getEstimatedMxy());
9973         assertNull(calibrator.getEstimatedMxz());
9974         assertNull(calibrator.getEstimatedMyx());
9975         assertNull(calibrator.getEstimatedMyz());
9976         assertNull(calibrator.getEstimatedMzx());
9977         assertNull(calibrator.getEstimatedMzy());
9978         assertNull(calibrator.getEstimatedCovariance());
9979         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9980         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9981                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9982         assertNotNull(calibrator.getGroundTruthGravityNorm());
9983         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9984         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9985         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9986                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9987         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9988         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9989         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9990     }
9991 
9992     @Test
9993     public void testConstructor86() throws WrongSizeException {
9994         final Collection<StandardDeviationBodyKinematics> measurements =
9995                 Collections.emptyList();
9996         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9997         final double latitude = Math.toRadians(
9998                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9999         final double longitude = Math.toRadians(
10000                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10001         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10002         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10003         final NEDVelocity nedVelocity = new NEDVelocity();
10004         final ECEFPosition ecefPosition = new ECEFPosition();
10005         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10006         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10007                 ecefPosition, ecefVelocity);
10008 
10009         final KnownPositionAccelerometerCalibrator calibrator =
10010                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10011                         true, this);
10012 
10013         // check default values
10014         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
10015         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
10016         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
10017         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10018         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
10019         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10020         final Acceleration bx2 = new Acceleration(0.0,
10021                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10022         calibrator.getInitialBiasXAsAcceleration(bx2);
10023         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
10024         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10025         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10026         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
10027         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10028         final Acceleration by2 = new Acceleration(0.0,
10029                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10030         calibrator.getInitialBiasYAsAcceleration(by2);
10031         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
10032         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10033         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10034         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
10035         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10036         final Acceleration bz2 = new Acceleration(0.0,
10037                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10038         calibrator.getInitialBiasZAsAcceleration(bz2);
10039         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
10040         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10041         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10042         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10043         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10044         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10045         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10046         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10047         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10048         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10049         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10050         final double[] bias1 = calibrator.getInitialBias();
10051         assertArrayEquals(bias1, new double[3], 0.0);
10052         final double[] bias2 = new double[3];
10053         calibrator.getInitialBias(bias2);
10054         assertArrayEquals(bias1, bias2, 0.0);
10055         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10056         assertEquals(b1, new Matrix(3, 1));
10057         final Matrix b2 = new Matrix(3, 1);
10058         calibrator.getInitialBiasAsMatrix(b2);
10059         assertEquals(b1, b2);
10060         final Matrix ma1 = calibrator.getInitialMa();
10061         assertEquals(ma1, new Matrix(3, 3));
10062         final Matrix ma2 = new Matrix(3, 3);
10063         calibrator.getInitialMa(ma2);
10064         assertEquals(ma1, ma2);
10065         assertSame(calibrator.getEcefPosition(), ecefPosition);
10066         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10067         final NEDPosition nedPosition1 = new NEDPosition();
10068         assertTrue(calibrator.getNedPosition(nedPosition1));
10069         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10070         assertSame(calibrator.getMeasurements(), measurements);
10071         assertTrue(calibrator.isCommonAxisUsed());
10072         assertSame(calibrator.getListener(), this);
10073         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10074         assertFalse(calibrator.isReady());
10075         assertFalse(calibrator.isRunning());
10076         assertNull(calibrator.getEstimatedBiases());
10077         assertFalse(calibrator.getEstimatedBiases(null));
10078         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10079         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10080         assertNull(calibrator.getEstimatedBiasFx());
10081         assertNull(calibrator.getEstimatedBiasFy());
10082         assertNull(calibrator.getEstimatedBiasFz());
10083         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10084         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10085         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10086         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10087         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10088         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10089         assertNull(calibrator.getEstimatedMa());
10090         assertNull(calibrator.getEstimatedSx());
10091         assertNull(calibrator.getEstimatedSy());
10092         assertNull(calibrator.getEstimatedSz());
10093         assertNull(calibrator.getEstimatedMxy());
10094         assertNull(calibrator.getEstimatedMxz());
10095         assertNull(calibrator.getEstimatedMyx());
10096         assertNull(calibrator.getEstimatedMyz());
10097         assertNull(calibrator.getEstimatedMzx());
10098         assertNull(calibrator.getEstimatedMzy());
10099         assertNull(calibrator.getEstimatedCovariance());
10100         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10101         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10102                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10103         assertNotNull(calibrator.getGroundTruthGravityNorm());
10104         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10105         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10106         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10107                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10108         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10109         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10110         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10111     }
10112 
10113     @Test
10114     public void testConstructor87() throws WrongSizeException {
10115         final Matrix ba = generateBa();
10116         final double biasX = ba.getElementAtIndex(0);
10117         final double biasY = ba.getElementAtIndex(1);
10118         final double biasZ = ba.getElementAtIndex(2);
10119 
10120         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10121         final double latitude = Math.toRadians(
10122                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10123         final double longitude = Math.toRadians(
10124                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10125         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10126         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10127         final NEDVelocity nedVelocity = new NEDVelocity();
10128         final ECEFPosition ecefPosition = new ECEFPosition();
10129         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10130         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10131                 ecefPosition, ecefVelocity);
10132 
10133         final KnownPositionAccelerometerCalibrator calibrator =
10134                 new KnownPositionAccelerometerCalibrator(ecefPosition,
10135                         biasX, biasY, biasZ);
10136 
10137         // check default values
10138         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10139         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10140         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10141         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10142         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10143         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10144         final Acceleration bx2 = new Acceleration(0.0,
10145                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10146         calibrator.getInitialBiasXAsAcceleration(bx2);
10147         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10148         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10149         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10150         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10151         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10152         final Acceleration by2 = new Acceleration(0.0,
10153                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10154         calibrator.getInitialBiasYAsAcceleration(by2);
10155         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10156         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10157         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10158         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10159         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10160         final Acceleration bz2 = new Acceleration(0.0,
10161                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10162         calibrator.getInitialBiasZAsAcceleration(bz2);
10163         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10164         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10165         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10166         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10167         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10168         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10169         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10170         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10171         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10172         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10173         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10174         final double[] bias1 = calibrator.getInitialBias();
10175         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10176         final double[] bias2 = new double[3];
10177         calibrator.getInitialBias(bias2);
10178         assertArrayEquals(bias1, bias2, 0.0);
10179         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10180         assertEquals(b1, ba);
10181         final Matrix b2 = new Matrix(3, 1);
10182         calibrator.getInitialBiasAsMatrix(b2);
10183         assertEquals(b1, b2);
10184         final Matrix ma1 = calibrator.getInitialMa();
10185         assertEquals(ma1, new Matrix(3, 3));
10186         final Matrix ma2 = new Matrix(3, 3);
10187         calibrator.getInitialMa(ma2);
10188         assertEquals(ma1, ma2);
10189         assertSame(calibrator.getEcefPosition(), ecefPosition);
10190         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10191         final NEDPosition nedPosition1 = new NEDPosition();
10192         assertTrue(calibrator.getNedPosition(nedPosition1));
10193         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10194         assertNull(calibrator.getMeasurements());
10195         assertFalse(calibrator.isCommonAxisUsed());
10196         assertNull(calibrator.getListener());
10197         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10198         assertFalse(calibrator.isReady());
10199         assertFalse(calibrator.isRunning());
10200         assertNull(calibrator.getEstimatedBiases());
10201         assertFalse(calibrator.getEstimatedBiases(null));
10202         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10203         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10204         assertNull(calibrator.getEstimatedBiasFx());
10205         assertNull(calibrator.getEstimatedBiasFy());
10206         assertNull(calibrator.getEstimatedBiasFz());
10207         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10208         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10209         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10210         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10211         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10212         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10213         assertNull(calibrator.getEstimatedMa());
10214         assertNull(calibrator.getEstimatedSx());
10215         assertNull(calibrator.getEstimatedSy());
10216         assertNull(calibrator.getEstimatedSz());
10217         assertNull(calibrator.getEstimatedMxy());
10218         assertNull(calibrator.getEstimatedMxz());
10219         assertNull(calibrator.getEstimatedMyx());
10220         assertNull(calibrator.getEstimatedMyz());
10221         assertNull(calibrator.getEstimatedMzx());
10222         assertNull(calibrator.getEstimatedMzy());
10223         assertNull(calibrator.getEstimatedCovariance());
10224         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10225         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10226                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10227         assertNotNull(calibrator.getGroundTruthGravityNorm());
10228         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10229         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10230         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10231                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10232         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10233         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10234         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10235     }
10236 
10237     @Test
10238     public void testConstructor88() throws WrongSizeException {
10239         final Matrix ba = generateBa();
10240         final double biasX = ba.getElementAtIndex(0);
10241         final double biasY = ba.getElementAtIndex(1);
10242         final double biasZ = ba.getElementAtIndex(2);
10243 
10244         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10245         final double latitude = Math.toRadians(
10246                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10247         final double longitude = Math.toRadians(
10248                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10249         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10250         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10251         final NEDVelocity nedVelocity = new NEDVelocity();
10252         final ECEFPosition ecefPosition = new ECEFPosition();
10253         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10254         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10255                 ecefPosition, ecefVelocity);
10256 
10257         final KnownPositionAccelerometerCalibrator calibrator =
10258                 new KnownPositionAccelerometerCalibrator(ecefPosition,
10259                         biasX, biasY, biasZ, this);
10260 
10261         // check default values
10262         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10263         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10264         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10265         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10266         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10267         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10268         final Acceleration bx2 = new Acceleration(0.0,
10269                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10270         calibrator.getInitialBiasXAsAcceleration(bx2);
10271         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10272         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10273         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10274         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10275         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10276         final Acceleration by2 = new Acceleration(0.0,
10277                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10278         calibrator.getInitialBiasYAsAcceleration(by2);
10279         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10280         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10281         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10282         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10283         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10284         final Acceleration bz2 = new Acceleration(0.0,
10285                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10286         calibrator.getInitialBiasZAsAcceleration(bz2);
10287         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10288         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10289         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10290         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10291         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10292         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10293         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10294         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10295         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10296         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10297         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10298         final double[] bias1 = calibrator.getInitialBias();
10299         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10300         final double[] bias2 = new double[3];
10301         calibrator.getInitialBias(bias2);
10302         assertArrayEquals(bias1, bias2, 0.0);
10303         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10304         assertEquals(b1, ba);
10305         final Matrix b2 = new Matrix(3, 1);
10306         calibrator.getInitialBiasAsMatrix(b2);
10307         assertEquals(b1, b2);
10308         final Matrix ma1 = calibrator.getInitialMa();
10309         assertEquals(ma1, new Matrix(3, 3));
10310         final Matrix ma2 = new Matrix(3, 3);
10311         calibrator.getInitialMa(ma2);
10312         assertEquals(ma1, ma2);
10313         assertSame(calibrator.getEcefPosition(), ecefPosition);
10314         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10315         final NEDPosition nedPosition1 = new NEDPosition();
10316         assertTrue(calibrator.getNedPosition(nedPosition1));
10317         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10318         assertNull(calibrator.getMeasurements());
10319         assertFalse(calibrator.isCommonAxisUsed());
10320         assertSame(calibrator.getListener(), this);
10321         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10322         assertFalse(calibrator.isReady());
10323         assertFalse(calibrator.isRunning());
10324         assertNull(calibrator.getEstimatedBiases());
10325         assertFalse(calibrator.getEstimatedBiases(null));
10326         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10327         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10328         assertNull(calibrator.getEstimatedBiasFx());
10329         assertNull(calibrator.getEstimatedBiasFy());
10330         assertNull(calibrator.getEstimatedBiasFz());
10331         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10332         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10333         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10334         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10335         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10336         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10337         assertNull(calibrator.getEstimatedMa());
10338         assertNull(calibrator.getEstimatedSx());
10339         assertNull(calibrator.getEstimatedSy());
10340         assertNull(calibrator.getEstimatedSz());
10341         assertNull(calibrator.getEstimatedMxy());
10342         assertNull(calibrator.getEstimatedMxz());
10343         assertNull(calibrator.getEstimatedMyx());
10344         assertNull(calibrator.getEstimatedMyz());
10345         assertNull(calibrator.getEstimatedMzx());
10346         assertNull(calibrator.getEstimatedMzy());
10347         assertNull(calibrator.getEstimatedCovariance());
10348         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10349         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10350                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10351         assertNotNull(calibrator.getGroundTruthGravityNorm());
10352         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10353         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10354         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10355                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10356         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10357         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10358         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10359     }
10360 
10361     @Test
10362     public void testConstructor89() throws WrongSizeException {
10363         final Collection<StandardDeviationBodyKinematics> measurements =
10364                 Collections.emptyList();
10365 
10366         final Matrix ba = generateBa();
10367         final double biasX = ba.getElementAtIndex(0);
10368         final double biasY = ba.getElementAtIndex(1);
10369         final double biasZ = ba.getElementAtIndex(2);
10370 
10371         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10372         final double latitude = Math.toRadians(
10373                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10374         final double longitude = Math.toRadians(
10375                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10376         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10377         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10378         final NEDVelocity nedVelocity = new NEDVelocity();
10379         final ECEFPosition ecefPosition = new ECEFPosition();
10380         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10381         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10382                 ecefPosition, ecefVelocity);
10383 
10384         final KnownPositionAccelerometerCalibrator calibrator =
10385                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10386                         biasX, biasY, biasZ);
10387 
10388         // check default values
10389         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10390         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10391         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10392         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10393         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10394         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10395         final Acceleration bx2 = new Acceleration(0.0,
10396                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10397         calibrator.getInitialBiasXAsAcceleration(bx2);
10398         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10399         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10400         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10401         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10402         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10403         final Acceleration by2 = new Acceleration(0.0,
10404                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10405         calibrator.getInitialBiasYAsAcceleration(by2);
10406         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10407         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10408         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10409         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10410         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10411         final Acceleration bz2 = new Acceleration(0.0,
10412                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10413         calibrator.getInitialBiasZAsAcceleration(bz2);
10414         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10415         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10416         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10417         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10418         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10419         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10420         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10421         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10422         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10423         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10424         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10425         final double[] bias1 = calibrator.getInitialBias();
10426         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10427         final double[] bias2 = new double[3];
10428         calibrator.getInitialBias(bias2);
10429         assertArrayEquals(bias1, bias2, 0.0);
10430         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10431         assertEquals(b1, ba);
10432         final Matrix b2 = new Matrix(3, 1);
10433         calibrator.getInitialBiasAsMatrix(b2);
10434         assertEquals(b1, b2);
10435         final Matrix ma1 = calibrator.getInitialMa();
10436         assertEquals(ma1, new Matrix(3, 3));
10437         final Matrix ma2 = new Matrix(3, 3);
10438         calibrator.getInitialMa(ma2);
10439         assertEquals(ma1, ma2);
10440         assertSame(calibrator.getEcefPosition(), ecefPosition);
10441         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10442         final NEDPosition nedPosition1 = new NEDPosition();
10443         assertTrue(calibrator.getNedPosition(nedPosition1));
10444         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10445         assertSame(calibrator.getMeasurements(), measurements);
10446         assertFalse(calibrator.isCommonAxisUsed());
10447         assertNull(calibrator.getListener());
10448         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10449         assertFalse(calibrator.isReady());
10450         assertFalse(calibrator.isRunning());
10451         assertNull(calibrator.getEstimatedBiases());
10452         assertFalse(calibrator.getEstimatedBiases(null));
10453         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10454         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10455         assertNull(calibrator.getEstimatedBiasFx());
10456         assertNull(calibrator.getEstimatedBiasFy());
10457         assertNull(calibrator.getEstimatedBiasFz());
10458         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10459         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10460         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10461         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10462         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10463         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10464         assertNull(calibrator.getEstimatedMa());
10465         assertNull(calibrator.getEstimatedSx());
10466         assertNull(calibrator.getEstimatedSy());
10467         assertNull(calibrator.getEstimatedSz());
10468         assertNull(calibrator.getEstimatedMxy());
10469         assertNull(calibrator.getEstimatedMxz());
10470         assertNull(calibrator.getEstimatedMyx());
10471         assertNull(calibrator.getEstimatedMyz());
10472         assertNull(calibrator.getEstimatedMzx());
10473         assertNull(calibrator.getEstimatedMzy());
10474         assertNull(calibrator.getEstimatedCovariance());
10475         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10476         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10477                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10478         assertNotNull(calibrator.getGroundTruthGravityNorm());
10479         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10480         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10481         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10482                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10483         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10484         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10485         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10486     }
10487 
10488     @Test
10489     public void testConstructor90() throws WrongSizeException {
10490         final Collection<StandardDeviationBodyKinematics> measurements =
10491                 Collections.emptyList();
10492 
10493         final Matrix ba = generateBa();
10494         final double biasX = ba.getElementAtIndex(0);
10495         final double biasY = ba.getElementAtIndex(1);
10496         final double biasZ = ba.getElementAtIndex(2);
10497 
10498         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10499         final double latitude = Math.toRadians(
10500                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10501         final double longitude = Math.toRadians(
10502                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10503         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10504         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10505         final NEDVelocity nedVelocity = new NEDVelocity();
10506         final ECEFPosition ecefPosition = new ECEFPosition();
10507         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10508         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10509                 ecefPosition, ecefVelocity);
10510 
10511         final KnownPositionAccelerometerCalibrator calibrator =
10512                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10513                         biasX, biasY, biasZ, this);
10514 
10515         // check default values
10516         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10517         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10518         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10519         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10520         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10521         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10522         final Acceleration bx2 = new Acceleration(0.0,
10523                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10524         calibrator.getInitialBiasXAsAcceleration(bx2);
10525         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10526         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10527         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10528         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10529         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10530         final Acceleration by2 = new Acceleration(0.0,
10531                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10532         calibrator.getInitialBiasYAsAcceleration(by2);
10533         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10534         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10535         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10536         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10537         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10538         final Acceleration bz2 = new Acceleration(0.0,
10539                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10540         calibrator.getInitialBiasZAsAcceleration(bz2);
10541         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10542         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10543         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10544         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10545         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10546         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10547         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10548         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10549         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10550         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10551         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10552         final double[] bias1 = calibrator.getInitialBias();
10553         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10554         final double[] bias2 = new double[3];
10555         calibrator.getInitialBias(bias2);
10556         assertArrayEquals(bias1, bias2, 0.0);
10557         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10558         assertEquals(b1, ba);
10559         final Matrix b2 = new Matrix(3, 1);
10560         calibrator.getInitialBiasAsMatrix(b2);
10561         assertEquals(b1, b2);
10562         final Matrix ma1 = calibrator.getInitialMa();
10563         assertEquals(ma1, new Matrix(3, 3));
10564         final Matrix ma2 = new Matrix(3, 3);
10565         calibrator.getInitialMa(ma2);
10566         assertEquals(ma1, ma2);
10567         assertSame(calibrator.getEcefPosition(), ecefPosition);
10568         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10569         final NEDPosition nedPosition1 = new NEDPosition();
10570         assertTrue(calibrator.getNedPosition(nedPosition1));
10571         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10572         assertSame(calibrator.getMeasurements(), measurements);
10573         assertFalse(calibrator.isCommonAxisUsed());
10574         assertSame(calibrator.getListener(), this);
10575         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10576         assertFalse(calibrator.isReady());
10577         assertFalse(calibrator.isRunning());
10578         assertNull(calibrator.getEstimatedBiases());
10579         assertFalse(calibrator.getEstimatedBiases(null));
10580         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10581         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10582         assertNull(calibrator.getEstimatedBiasFx());
10583         assertNull(calibrator.getEstimatedBiasFy());
10584         assertNull(calibrator.getEstimatedBiasFz());
10585         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10586         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10587         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10588         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10589         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10590         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10591         assertNull(calibrator.getEstimatedMa());
10592         assertNull(calibrator.getEstimatedSx());
10593         assertNull(calibrator.getEstimatedSy());
10594         assertNull(calibrator.getEstimatedSz());
10595         assertNull(calibrator.getEstimatedMxy());
10596         assertNull(calibrator.getEstimatedMxz());
10597         assertNull(calibrator.getEstimatedMyx());
10598         assertNull(calibrator.getEstimatedMyz());
10599         assertNull(calibrator.getEstimatedMzx());
10600         assertNull(calibrator.getEstimatedMzy());
10601         assertNull(calibrator.getEstimatedCovariance());
10602         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10603         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10604                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10605         assertNotNull(calibrator.getGroundTruthGravityNorm());
10606         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10607         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10608         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10609                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10610         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10611         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10612         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10613     }
10614 
10615     @Test
10616     public void testConstructor91() throws WrongSizeException {
10617         final Matrix ba = generateBa();
10618         final double biasX = ba.getElementAtIndex(0);
10619         final double biasY = ba.getElementAtIndex(1);
10620         final double biasZ = ba.getElementAtIndex(2);
10621 
10622         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10623         final double latitude = Math.toRadians(
10624                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10625         final double longitude = Math.toRadians(
10626                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10627         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10628         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10629         final NEDVelocity nedVelocity = new NEDVelocity();
10630         final ECEFPosition ecefPosition = new ECEFPosition();
10631         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10632         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10633                 ecefPosition, ecefVelocity);
10634 
10635         final KnownPositionAccelerometerCalibrator calibrator =
10636                 new KnownPositionAccelerometerCalibrator(ecefPosition,
10637                         true, biasX, biasY, biasZ);
10638 
10639         // check default values
10640         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10641         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10642         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10643         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10644         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10645         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10646         final Acceleration bx2 = new Acceleration(0.0,
10647                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10648         calibrator.getInitialBiasXAsAcceleration(bx2);
10649         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10650         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10651         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10652         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10653         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10654         final Acceleration by2 = new Acceleration(0.0,
10655                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10656         calibrator.getInitialBiasYAsAcceleration(by2);
10657         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10658         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10659         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10660         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10661         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10662         final Acceleration bz2 = new Acceleration(0.0,
10663                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10664         calibrator.getInitialBiasZAsAcceleration(bz2);
10665         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10666         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10667         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10668         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10669         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10670         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10671         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10672         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10673         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10674         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10675         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10676         final double[] bias1 = calibrator.getInitialBias();
10677         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10678         final double[] bias2 = new double[3];
10679         calibrator.getInitialBias(bias2);
10680         assertArrayEquals(bias1, bias2, 0.0);
10681         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10682         assertEquals(b1, ba);
10683         final Matrix b2 = new Matrix(3, 1);
10684         calibrator.getInitialBiasAsMatrix(b2);
10685         assertEquals(b1, b2);
10686         final Matrix ma1 = calibrator.getInitialMa();
10687         assertEquals(ma1, new Matrix(3, 3));
10688         final Matrix ma2 = new Matrix(3, 3);
10689         calibrator.getInitialMa(ma2);
10690         assertEquals(ma1, ma2);
10691         assertSame(calibrator.getEcefPosition(), ecefPosition);
10692         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10693         final NEDPosition nedPosition1 = new NEDPosition();
10694         assertTrue(calibrator.getNedPosition(nedPosition1));
10695         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10696         assertNull(calibrator.getMeasurements());
10697         assertTrue(calibrator.isCommonAxisUsed());
10698         assertNull(calibrator.getListener());
10699         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10700         assertFalse(calibrator.isReady());
10701         assertFalse(calibrator.isRunning());
10702         assertNull(calibrator.getEstimatedBiases());
10703         assertFalse(calibrator.getEstimatedBiases(null));
10704         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10705         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10706         assertNull(calibrator.getEstimatedBiasFx());
10707         assertNull(calibrator.getEstimatedBiasFy());
10708         assertNull(calibrator.getEstimatedBiasFz());
10709         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10710         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10711         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10712         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10713         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10714         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10715         assertNull(calibrator.getEstimatedMa());
10716         assertNull(calibrator.getEstimatedSx());
10717         assertNull(calibrator.getEstimatedSy());
10718         assertNull(calibrator.getEstimatedSz());
10719         assertNull(calibrator.getEstimatedMxy());
10720         assertNull(calibrator.getEstimatedMxz());
10721         assertNull(calibrator.getEstimatedMyx());
10722         assertNull(calibrator.getEstimatedMyz());
10723         assertNull(calibrator.getEstimatedMzx());
10724         assertNull(calibrator.getEstimatedMzy());
10725         assertNull(calibrator.getEstimatedCovariance());
10726         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10727         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10728                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10729         assertNotNull(calibrator.getGroundTruthGravityNorm());
10730         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10731         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10732         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10733                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10734         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10735         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10736         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10737     }
10738 
10739     @Test
10740     public void testConstructor92() throws WrongSizeException {
10741         final Matrix ba = generateBa();
10742         final double biasX = ba.getElementAtIndex(0);
10743         final double biasY = ba.getElementAtIndex(1);
10744         final double biasZ = ba.getElementAtIndex(2);
10745 
10746         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10747         final double latitude = Math.toRadians(
10748                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10749         final double longitude = Math.toRadians(
10750                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10751         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10752         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10753         final NEDVelocity nedVelocity = new NEDVelocity();
10754         final ECEFPosition ecefPosition = new ECEFPosition();
10755         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10756         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10757                 ecefPosition, ecefVelocity);
10758 
10759         final KnownPositionAccelerometerCalibrator calibrator =
10760                 new KnownPositionAccelerometerCalibrator(ecefPosition,
10761                         true, biasX, biasY, biasZ, this);
10762 
10763         // check default values
10764         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10765         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10766         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10767         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10768         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10769         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10770         final Acceleration bx2 = new Acceleration(0.0,
10771                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10772         calibrator.getInitialBiasXAsAcceleration(bx2);
10773         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10774         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10775         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10776         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10777         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10778         final Acceleration by2 = new Acceleration(0.0,
10779                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10780         calibrator.getInitialBiasYAsAcceleration(by2);
10781         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10782         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10783         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10784         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10785         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10786         final Acceleration bz2 = new Acceleration(0.0,
10787                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10788         calibrator.getInitialBiasZAsAcceleration(bz2);
10789         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10790         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10791         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10792         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10793         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10794         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10795         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10796         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10797         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10798         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10799         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10800         final double[] bias1 = calibrator.getInitialBias();
10801         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10802         final double[] bias2 = new double[3];
10803         calibrator.getInitialBias(bias2);
10804         assertArrayEquals(bias1, bias2, 0.0);
10805         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10806         assertEquals(b1, ba);
10807         final Matrix b2 = new Matrix(3, 1);
10808         calibrator.getInitialBiasAsMatrix(b2);
10809         assertEquals(b1, b2);
10810         final Matrix ma1 = calibrator.getInitialMa();
10811         assertEquals(ma1, new Matrix(3, 3));
10812         final Matrix ma2 = new Matrix(3, 3);
10813         calibrator.getInitialMa(ma2);
10814         assertEquals(ma1, ma2);
10815         assertSame(calibrator.getEcefPosition(), ecefPosition);
10816         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10817         final NEDPosition nedPosition1 = new NEDPosition();
10818         assertTrue(calibrator.getNedPosition(nedPosition1));
10819         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10820         assertNull(calibrator.getMeasurements());
10821         assertTrue(calibrator.isCommonAxisUsed());
10822         assertSame(calibrator.getListener(), this);
10823         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10824         assertFalse(calibrator.isReady());
10825         assertFalse(calibrator.isRunning());
10826         assertNull(calibrator.getEstimatedBiases());
10827         assertFalse(calibrator.getEstimatedBiases(null));
10828         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10829         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10830         assertNull(calibrator.getEstimatedBiasFx());
10831         assertNull(calibrator.getEstimatedBiasFy());
10832         assertNull(calibrator.getEstimatedBiasFz());
10833         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10834         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10835         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10836         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10837         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10838         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10839         assertNull(calibrator.getEstimatedMa());
10840         assertNull(calibrator.getEstimatedSx());
10841         assertNull(calibrator.getEstimatedSy());
10842         assertNull(calibrator.getEstimatedSz());
10843         assertNull(calibrator.getEstimatedMxy());
10844         assertNull(calibrator.getEstimatedMxz());
10845         assertNull(calibrator.getEstimatedMyx());
10846         assertNull(calibrator.getEstimatedMyz());
10847         assertNull(calibrator.getEstimatedMzx());
10848         assertNull(calibrator.getEstimatedMzy());
10849         assertNull(calibrator.getEstimatedCovariance());
10850         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10851         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10852                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10853         assertNotNull(calibrator.getGroundTruthGravityNorm());
10854         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10855         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10856         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10857                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10858         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10859         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10860         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10861     }
10862 
10863     @Test
10864     public void testConstructor93() throws WrongSizeException {
10865         final Collection<StandardDeviationBodyKinematics> measurements =
10866                 Collections.emptyList();
10867 
10868         final Matrix ba = generateBa();
10869         final double biasX = ba.getElementAtIndex(0);
10870         final double biasY = ba.getElementAtIndex(1);
10871         final double biasZ = ba.getElementAtIndex(2);
10872 
10873         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10874         final double latitude = Math.toRadians(
10875                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10876         final double longitude = Math.toRadians(
10877                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10878         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10879         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10880         final NEDVelocity nedVelocity = new NEDVelocity();
10881         final ECEFPosition ecefPosition = new ECEFPosition();
10882         final ECEFVelocity ecefVelocity = new ECEFVelocity();
10883         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10884                 ecefPosition, ecefVelocity);
10885 
10886         final KnownPositionAccelerometerCalibrator calibrator =
10887                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10888                         true, biasX, biasY, biasZ);
10889 
10890         // check default values
10891         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10892         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10893         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10894         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10895         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10896         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10897         final Acceleration bx2 = new Acceleration(0.0,
10898                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10899         calibrator.getInitialBiasXAsAcceleration(bx2);
10900         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10901         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10902         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10903         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10904         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10905         final Acceleration by2 = new Acceleration(0.0,
10906                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10907         calibrator.getInitialBiasYAsAcceleration(by2);
10908         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10909         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10910         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10911         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10912         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10913         final Acceleration bz2 = new Acceleration(0.0,
10914                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10915         calibrator.getInitialBiasZAsAcceleration(bz2);
10916         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10917         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10918         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10919         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10920         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10921         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10922         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10923         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10924         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10925         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10926         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10927         final double[] bias1 = calibrator.getInitialBias();
10928         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10929         final double[] bias2 = new double[3];
10930         calibrator.getInitialBias(bias2);
10931         assertArrayEquals(bias1, bias2, 0.0);
10932         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10933         assertEquals(b1, ba);
10934         final Matrix b2 = new Matrix(3, 1);
10935         calibrator.getInitialBiasAsMatrix(b2);
10936         assertEquals(b1, b2);
10937         final Matrix ma1 = calibrator.getInitialMa();
10938         assertEquals(ma1, new Matrix(3, 3));
10939         final Matrix ma2 = new Matrix(3, 3);
10940         calibrator.getInitialMa(ma2);
10941         assertEquals(ma1, ma2);
10942         assertSame(calibrator.getEcefPosition(), ecefPosition);
10943         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10944         final NEDPosition nedPosition1 = new NEDPosition();
10945         assertTrue(calibrator.getNedPosition(nedPosition1));
10946         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10947         assertSame(calibrator.getMeasurements(), measurements);
10948         assertTrue(calibrator.isCommonAxisUsed());
10949         assertNull(calibrator.getListener());
10950         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10951         assertFalse(calibrator.isReady());
10952         assertFalse(calibrator.isRunning());
10953         assertNull(calibrator.getEstimatedBiases());
10954         assertFalse(calibrator.getEstimatedBiases(null));
10955         assertNull(calibrator.getEstimatedBiasesAsMatrix());
10956         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10957         assertNull(calibrator.getEstimatedBiasFx());
10958         assertNull(calibrator.getEstimatedBiasFy());
10959         assertNull(calibrator.getEstimatedBiasFz());
10960         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10961         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10962         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10963         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10964         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10965         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10966         assertNull(calibrator.getEstimatedMa());
10967         assertNull(calibrator.getEstimatedSx());
10968         assertNull(calibrator.getEstimatedSy());
10969         assertNull(calibrator.getEstimatedSz());
10970         assertNull(calibrator.getEstimatedMxy());
10971         assertNull(calibrator.getEstimatedMxz());
10972         assertNull(calibrator.getEstimatedMyx());
10973         assertNull(calibrator.getEstimatedMyz());
10974         assertNull(calibrator.getEstimatedMzx());
10975         assertNull(calibrator.getEstimatedMzy());
10976         assertNull(calibrator.getEstimatedCovariance());
10977         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10978         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10979                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10980         assertNotNull(calibrator.getGroundTruthGravityNorm());
10981         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10982         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10983         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10984                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10985         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10986         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10987         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10988     }
10989 
10990     @Test
10991     public void testConstructor94() throws WrongSizeException {
10992         final Collection<StandardDeviationBodyKinematics> measurements =
10993                 Collections.emptyList();
10994 
10995         final Matrix ba = generateBa();
10996         final double biasX = ba.getElementAtIndex(0);
10997         final double biasY = ba.getElementAtIndex(1);
10998         final double biasZ = ba.getElementAtIndex(2);
10999 
11000         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11001         final double latitude = Math.toRadians(
11002                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11003         final double longitude = Math.toRadians(
11004                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11005         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11006         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11007         final NEDVelocity nedVelocity = new NEDVelocity();
11008         final ECEFPosition ecefPosition = new ECEFPosition();
11009         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11010         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11011                 ecefPosition, ecefVelocity);
11012 
11013         final KnownPositionAccelerometerCalibrator calibrator =
11014                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11015                         true, biasX, biasY, biasZ, this);
11016 
11017         // check default values
11018         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11019         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11020         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11021         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11022         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11023         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11024         final Acceleration bx2 = new Acceleration(0.0,
11025                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11026         calibrator.getInitialBiasXAsAcceleration(bx2);
11027         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11028         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11029         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11030         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11031         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11032         final Acceleration by2 = new Acceleration(0.0,
11033                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11034         calibrator.getInitialBiasYAsAcceleration(by2);
11035         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11036         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11037         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11038         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11039         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11040         final Acceleration bz2 = new Acceleration(0.0,
11041                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11042         calibrator.getInitialBiasZAsAcceleration(bz2);
11043         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11044         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11045         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11046         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11047         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11048         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11049         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11050         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11051         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11052         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11053         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11054         final double[] bias1 = calibrator.getInitialBias();
11055         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11056         final double[] bias2 = new double[3];
11057         calibrator.getInitialBias(bias2);
11058         assertArrayEquals(bias1, bias2, 0.0);
11059         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11060         assertEquals(b1, ba);
11061         final Matrix b2 = new Matrix(3, 1);
11062         calibrator.getInitialBiasAsMatrix(b2);
11063         assertEquals(b1, b2);
11064         final Matrix ma1 = calibrator.getInitialMa();
11065         assertEquals(ma1, new Matrix(3, 3));
11066         final Matrix ma2 = new Matrix(3, 3);
11067         calibrator.getInitialMa(ma2);
11068         assertEquals(ma1, ma2);
11069         assertSame(calibrator.getEcefPosition(), ecefPosition);
11070         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11071         final NEDPosition nedPosition1 = new NEDPosition();
11072         assertTrue(calibrator.getNedPosition(nedPosition1));
11073         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11074         assertSame(calibrator.getMeasurements(), measurements);
11075         assertTrue(calibrator.isCommonAxisUsed());
11076         assertSame(calibrator.getListener(), this);
11077         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11078         assertFalse(calibrator.isReady());
11079         assertFalse(calibrator.isRunning());
11080         assertNull(calibrator.getEstimatedBiases());
11081         assertFalse(calibrator.getEstimatedBiases(null));
11082         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11083         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11084         assertNull(calibrator.getEstimatedBiasFx());
11085         assertNull(calibrator.getEstimatedBiasFy());
11086         assertNull(calibrator.getEstimatedBiasFz());
11087         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11088         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11089         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11090         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11091         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11092         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11093         assertNull(calibrator.getEstimatedMa());
11094         assertNull(calibrator.getEstimatedSx());
11095         assertNull(calibrator.getEstimatedSy());
11096         assertNull(calibrator.getEstimatedSz());
11097         assertNull(calibrator.getEstimatedMxy());
11098         assertNull(calibrator.getEstimatedMxz());
11099         assertNull(calibrator.getEstimatedMyx());
11100         assertNull(calibrator.getEstimatedMyz());
11101         assertNull(calibrator.getEstimatedMzx());
11102         assertNull(calibrator.getEstimatedMzy());
11103         assertNull(calibrator.getEstimatedCovariance());
11104         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11105         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11106                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11107         assertNotNull(calibrator.getGroundTruthGravityNorm());
11108         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11109         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11110         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11111                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11112         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11113         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11114         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11115     }
11116 
11117     @Test
11118     public void testConstructor95() throws WrongSizeException {
11119         final Matrix ba = generateBa();
11120         final double biasX = ba.getElementAtIndex(0);
11121         final double biasY = ba.getElementAtIndex(1);
11122         final double biasZ = ba.getElementAtIndex(2);
11123 
11124         final Acceleration bx = new Acceleration(biasX,
11125                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11126         final Acceleration by = new Acceleration(biasY,
11127                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11128         final Acceleration bz = new Acceleration(biasZ,
11129                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11130 
11131         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11132         final double latitude = Math.toRadians(
11133                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11134         final double longitude = Math.toRadians(
11135                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11136         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11137         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11138         final NEDVelocity nedVelocity = new NEDVelocity();
11139         final ECEFPosition ecefPosition = new ECEFPosition();
11140         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11141         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11142                 ecefPosition, ecefVelocity);
11143 
11144         final KnownPositionAccelerometerCalibrator calibrator =
11145                 new KnownPositionAccelerometerCalibrator(ecefPosition,
11146                         bx, by, bz);
11147 
11148         // check default values
11149         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11150         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11151         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11152         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11153         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11154         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11155         final Acceleration bx2 = new Acceleration(0.0,
11156                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11157         calibrator.getInitialBiasXAsAcceleration(bx2);
11158         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11159         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11160         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11161         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11162         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11163         final Acceleration by2 = new Acceleration(0.0,
11164                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11165         calibrator.getInitialBiasYAsAcceleration(by2);
11166         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11167         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11168         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11169         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11170         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11171         final Acceleration bz2 = new Acceleration(0.0,
11172                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11173         calibrator.getInitialBiasZAsAcceleration(bz2);
11174         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11175         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11176         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11177         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11178         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11179         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11180         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11181         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11182         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11183         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11184         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11185         final double[] bias1 = calibrator.getInitialBias();
11186         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11187         final double[] bias2 = new double[3];
11188         calibrator.getInitialBias(bias2);
11189         assertArrayEquals(bias1, bias2, 0.0);
11190         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11191         assertEquals(b1, ba);
11192         final Matrix b2 = new Matrix(3, 1);
11193         calibrator.getInitialBiasAsMatrix(b2);
11194         assertEquals(b1, b2);
11195         final Matrix ma1 = calibrator.getInitialMa();
11196         assertEquals(ma1, new Matrix(3, 3));
11197         final Matrix ma2 = new Matrix(3, 3);
11198         calibrator.getInitialMa(ma2);
11199         assertEquals(ma1, ma2);
11200         assertSame(calibrator.getEcefPosition(), ecefPosition);
11201         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11202         final NEDPosition nedPosition1 = new NEDPosition();
11203         assertTrue(calibrator.getNedPosition(nedPosition1));
11204         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11205         assertNull(calibrator.getMeasurements());
11206         assertFalse(calibrator.isCommonAxisUsed());
11207         assertNull(calibrator.getListener());
11208         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11209         assertFalse(calibrator.isReady());
11210         assertFalse(calibrator.isRunning());
11211         assertNull(calibrator.getEstimatedBiases());
11212         assertFalse(calibrator.getEstimatedBiases(null));
11213         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11214         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11215         assertNull(calibrator.getEstimatedBiasFx());
11216         assertNull(calibrator.getEstimatedBiasFy());
11217         assertNull(calibrator.getEstimatedBiasFz());
11218         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11219         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11220         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11221         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11222         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11223         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11224         assertNull(calibrator.getEstimatedMa());
11225         assertNull(calibrator.getEstimatedSx());
11226         assertNull(calibrator.getEstimatedSy());
11227         assertNull(calibrator.getEstimatedSz());
11228         assertNull(calibrator.getEstimatedMxy());
11229         assertNull(calibrator.getEstimatedMxz());
11230         assertNull(calibrator.getEstimatedMyx());
11231         assertNull(calibrator.getEstimatedMyz());
11232         assertNull(calibrator.getEstimatedMzx());
11233         assertNull(calibrator.getEstimatedMzy());
11234         assertNull(calibrator.getEstimatedCovariance());
11235         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11236         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11237                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11238         assertNotNull(calibrator.getGroundTruthGravityNorm());
11239         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11240         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11241         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11242                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11243         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11244         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11245         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11246     }
11247 
11248     @Test
11249     public void testConstructor96() throws WrongSizeException {
11250         final Matrix ba = generateBa();
11251         final double biasX = ba.getElementAtIndex(0);
11252         final double biasY = ba.getElementAtIndex(1);
11253         final double biasZ = ba.getElementAtIndex(2);
11254 
11255         final Acceleration bx = new Acceleration(biasX,
11256                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11257         final Acceleration by = new Acceleration(biasY,
11258                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11259         final Acceleration bz = new Acceleration(biasZ,
11260                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11261 
11262         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11263         final double latitude = Math.toRadians(
11264                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11265         final double longitude = Math.toRadians(
11266                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11267         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11268         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11269         final NEDVelocity nedVelocity = new NEDVelocity();
11270         final ECEFPosition ecefPosition = new ECEFPosition();
11271         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11272         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11273                 ecefPosition, ecefVelocity);
11274 
11275         final KnownPositionAccelerometerCalibrator calibrator =
11276                 new KnownPositionAccelerometerCalibrator(ecefPosition,
11277                         bx, by, bz, this);
11278 
11279         // check default values
11280         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11281         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11282         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11283         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11284         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11285         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11286         final Acceleration bx2 = new Acceleration(0.0,
11287                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11288         calibrator.getInitialBiasXAsAcceleration(bx2);
11289         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11290         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11291         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11292         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11293         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11294         final Acceleration by2 = new Acceleration(0.0,
11295                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11296         calibrator.getInitialBiasYAsAcceleration(by2);
11297         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11298         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11299         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11300         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11301         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11302         final Acceleration bz2 = new Acceleration(0.0,
11303                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11304         calibrator.getInitialBiasZAsAcceleration(bz2);
11305         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11306         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11307         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11308         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11309         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11310         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11311         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11312         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11313         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11314         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11315         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11316         final double[] bias1 = calibrator.getInitialBias();
11317         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11318         final double[] bias2 = new double[3];
11319         calibrator.getInitialBias(bias2);
11320         assertArrayEquals(bias1, bias2, 0.0);
11321         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11322         assertEquals(b1, ba);
11323         final Matrix b2 = new Matrix(3, 1);
11324         calibrator.getInitialBiasAsMatrix(b2);
11325         assertEquals(b1, b2);
11326         final Matrix ma1 = calibrator.getInitialMa();
11327         assertEquals(ma1, new Matrix(3, 3));
11328         final Matrix ma2 = new Matrix(3, 3);
11329         calibrator.getInitialMa(ma2);
11330         assertEquals(ma1, ma2);
11331         assertSame(calibrator.getEcefPosition(), ecefPosition);
11332         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11333         final NEDPosition nedPosition1 = new NEDPosition();
11334         assertTrue(calibrator.getNedPosition(nedPosition1));
11335         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11336         assertNull(calibrator.getMeasurements());
11337         assertFalse(calibrator.isCommonAxisUsed());
11338         assertSame(calibrator.getListener(), this);
11339         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11340         assertFalse(calibrator.isReady());
11341         assertFalse(calibrator.isRunning());
11342         assertNull(calibrator.getEstimatedBiases());
11343         assertFalse(calibrator.getEstimatedBiases(null));
11344         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11345         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11346         assertNull(calibrator.getEstimatedBiasFx());
11347         assertNull(calibrator.getEstimatedBiasFy());
11348         assertNull(calibrator.getEstimatedBiasFz());
11349         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11350         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11351         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11352         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11353         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11354         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11355         assertNull(calibrator.getEstimatedMa());
11356         assertNull(calibrator.getEstimatedSx());
11357         assertNull(calibrator.getEstimatedSy());
11358         assertNull(calibrator.getEstimatedSz());
11359         assertNull(calibrator.getEstimatedMxy());
11360         assertNull(calibrator.getEstimatedMxz());
11361         assertNull(calibrator.getEstimatedMyx());
11362         assertNull(calibrator.getEstimatedMyz());
11363         assertNull(calibrator.getEstimatedMzx());
11364         assertNull(calibrator.getEstimatedMzy());
11365         assertNull(calibrator.getEstimatedCovariance());
11366         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11367         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11368                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11369         assertNotNull(calibrator.getGroundTruthGravityNorm());
11370         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11371         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11372         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11373                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11374         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11375         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11376         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11377     }
11378 
11379     @Test
11380     public void testConstructor97() throws WrongSizeException {
11381         final Collection<StandardDeviationBodyKinematics> measurements =
11382                 Collections.emptyList();
11383 
11384         final Matrix ba = generateBa();
11385         final double biasX = ba.getElementAtIndex(0);
11386         final double biasY = ba.getElementAtIndex(1);
11387         final double biasZ = ba.getElementAtIndex(2);
11388 
11389         final Acceleration bx = new Acceleration(biasX,
11390                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11391         final Acceleration by = new Acceleration(biasY,
11392                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11393         final Acceleration bz = new Acceleration(biasZ,
11394                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11395 
11396         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11397         final double latitude = Math.toRadians(
11398                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11399         final double longitude = Math.toRadians(
11400                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11401         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11402         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11403         final NEDVelocity nedVelocity = new NEDVelocity();
11404         final ECEFPosition ecefPosition = new ECEFPosition();
11405         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11406         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11407                 ecefPosition, ecefVelocity);
11408 
11409         final KnownPositionAccelerometerCalibrator calibrator =
11410                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11411                         bx, by, bz);
11412 
11413         // check default values
11414         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11415         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11416         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11417         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11418         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11419         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11420         final Acceleration bx2 = new Acceleration(0.0,
11421                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11422         calibrator.getInitialBiasXAsAcceleration(bx2);
11423         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11424         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11425         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11426         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11427         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11428         final Acceleration by2 = new Acceleration(0.0,
11429                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11430         calibrator.getInitialBiasYAsAcceleration(by2);
11431         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11432         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11433         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11434         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11435         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11436         final Acceleration bz2 = new Acceleration(0.0,
11437                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11438         calibrator.getInitialBiasZAsAcceleration(bz2);
11439         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11440         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11441         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11442         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11443         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11444         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11445         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11446         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11447         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11448         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11449         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11450         final double[] bias1 = calibrator.getInitialBias();
11451         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11452         final double[] bias2 = new double[3];
11453         calibrator.getInitialBias(bias2);
11454         assertArrayEquals(bias1, bias2, 0.0);
11455         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11456         assertEquals(b1, ba);
11457         final Matrix b2 = new Matrix(3, 1);
11458         calibrator.getInitialBiasAsMatrix(b2);
11459         assertEquals(b1, b2);
11460         final Matrix ma1 = calibrator.getInitialMa();
11461         assertEquals(ma1, new Matrix(3, 3));
11462         final Matrix ma2 = new Matrix(3, 3);
11463         calibrator.getInitialMa(ma2);
11464         assertEquals(ma1, ma2);
11465         assertSame(calibrator.getEcefPosition(), ecefPosition);
11466         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11467         final NEDPosition nedPosition1 = new NEDPosition();
11468         assertTrue(calibrator.getNedPosition(nedPosition1));
11469         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11470         assertSame(calibrator.getMeasurements(), measurements);
11471         assertFalse(calibrator.isCommonAxisUsed());
11472         assertNull(calibrator.getListener());
11473         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11474         assertFalse(calibrator.isReady());
11475         assertFalse(calibrator.isRunning());
11476         assertNull(calibrator.getEstimatedBiases());
11477         assertFalse(calibrator.getEstimatedBiases(null));
11478         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11479         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11480         assertNull(calibrator.getEstimatedBiasFx());
11481         assertNull(calibrator.getEstimatedBiasFy());
11482         assertNull(calibrator.getEstimatedBiasFz());
11483         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11484         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11485         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11486         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11487         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11488         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11489         assertNull(calibrator.getEstimatedMa());
11490         assertNull(calibrator.getEstimatedSx());
11491         assertNull(calibrator.getEstimatedSy());
11492         assertNull(calibrator.getEstimatedSz());
11493         assertNull(calibrator.getEstimatedMxy());
11494         assertNull(calibrator.getEstimatedMxz());
11495         assertNull(calibrator.getEstimatedMyx());
11496         assertNull(calibrator.getEstimatedMyz());
11497         assertNull(calibrator.getEstimatedMzx());
11498         assertNull(calibrator.getEstimatedMzy());
11499         assertNull(calibrator.getEstimatedCovariance());
11500         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11501         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11502                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11503         assertNotNull(calibrator.getGroundTruthGravityNorm());
11504         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11505         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11506         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11507                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11508         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11509         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11510         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11511     }
11512 
11513     @Test
11514     public void testConstructor98() throws WrongSizeException {
11515         final Collection<StandardDeviationBodyKinematics> measurements =
11516                 Collections.emptyList();
11517 
11518         final Matrix ba = generateBa();
11519         final double biasX = ba.getElementAtIndex(0);
11520         final double biasY = ba.getElementAtIndex(1);
11521         final double biasZ = ba.getElementAtIndex(2);
11522 
11523         final Acceleration bx = new Acceleration(biasX,
11524                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11525         final Acceleration by = new Acceleration(biasY,
11526                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11527         final Acceleration bz = new Acceleration(biasZ,
11528                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11529 
11530         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11531         final double latitude = Math.toRadians(
11532                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11533         final double longitude = Math.toRadians(
11534                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11535         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11536         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11537         final NEDVelocity nedVelocity = new NEDVelocity();
11538         final ECEFPosition ecefPosition = new ECEFPosition();
11539         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11540         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11541                 ecefPosition, ecefVelocity);
11542 
11543         final KnownPositionAccelerometerCalibrator calibrator =
11544                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11545                         bx, by, bz, this);
11546 
11547         // check default values
11548         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11549         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11550         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11551         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11552         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11553         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11554         final Acceleration bx2 = new Acceleration(0.0,
11555                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11556         calibrator.getInitialBiasXAsAcceleration(bx2);
11557         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11558         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11559         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11560         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11561         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11562         final Acceleration by2 = new Acceleration(0.0,
11563                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11564         calibrator.getInitialBiasYAsAcceleration(by2);
11565         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11566         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11567         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11568         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11569         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11570         final Acceleration bz2 = new Acceleration(0.0,
11571                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11572         calibrator.getInitialBiasZAsAcceleration(bz2);
11573         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11574         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11575         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11576         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11577         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11578         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11579         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11580         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11581         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11582         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11583         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11584         final double[] bias1 = calibrator.getInitialBias();
11585         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11586         final double[] bias2 = new double[3];
11587         calibrator.getInitialBias(bias2);
11588         assertArrayEquals(bias1, bias2, 0.0);
11589         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11590         assertEquals(b1, ba);
11591         final Matrix b2 = new Matrix(3, 1);
11592         calibrator.getInitialBiasAsMatrix(b2);
11593         assertEquals(b1, b2);
11594         final Matrix ma1 = calibrator.getInitialMa();
11595         assertEquals(ma1, new Matrix(3, 3));
11596         final Matrix ma2 = new Matrix(3, 3);
11597         calibrator.getInitialMa(ma2);
11598         assertEquals(ma1, ma2);
11599         assertSame(calibrator.getEcefPosition(), ecefPosition);
11600         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11601         final NEDPosition nedPosition1 = new NEDPosition();
11602         assertTrue(calibrator.getNedPosition(nedPosition1));
11603         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11604         assertSame(calibrator.getMeasurements(), measurements);
11605         assertFalse(calibrator.isCommonAxisUsed());
11606         assertSame(calibrator.getListener(), this);
11607         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11608         assertFalse(calibrator.isReady());
11609         assertFalse(calibrator.isRunning());
11610         assertNull(calibrator.getEstimatedBiases());
11611         assertFalse(calibrator.getEstimatedBiases(null));
11612         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11613         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11614         assertNull(calibrator.getEstimatedBiasFx());
11615         assertNull(calibrator.getEstimatedBiasFy());
11616         assertNull(calibrator.getEstimatedBiasFz());
11617         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11618         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11619         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11620         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11621         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11622         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11623         assertNull(calibrator.getEstimatedMa());
11624         assertNull(calibrator.getEstimatedSx());
11625         assertNull(calibrator.getEstimatedSy());
11626         assertNull(calibrator.getEstimatedSz());
11627         assertNull(calibrator.getEstimatedMxy());
11628         assertNull(calibrator.getEstimatedMxz());
11629         assertNull(calibrator.getEstimatedMyx());
11630         assertNull(calibrator.getEstimatedMyz());
11631         assertNull(calibrator.getEstimatedMzx());
11632         assertNull(calibrator.getEstimatedMzy());
11633         assertNull(calibrator.getEstimatedCovariance());
11634         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11635         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11636                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11637         assertNotNull(calibrator.getGroundTruthGravityNorm());
11638         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11639         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11640         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11641                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11642         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11643         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11644         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11645     }
11646 
11647     @Test
11648     public void testConstructor99() throws WrongSizeException {
11649         final Matrix ba = generateBa();
11650         final double biasX = ba.getElementAtIndex(0);
11651         final double biasY = ba.getElementAtIndex(1);
11652         final double biasZ = ba.getElementAtIndex(2);
11653 
11654         final Acceleration bx = new Acceleration(biasX,
11655                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11656         final Acceleration by = new Acceleration(biasY,
11657                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11658         final Acceleration bz = new Acceleration(biasZ,
11659                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11660 
11661         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11662         final double latitude = Math.toRadians(
11663                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11664         final double longitude = Math.toRadians(
11665                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11666         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11667         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11668         final NEDVelocity nedVelocity = new NEDVelocity();
11669         final ECEFPosition ecefPosition = new ECEFPosition();
11670         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11671         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11672                 ecefPosition, ecefVelocity);
11673 
11674         final KnownPositionAccelerometerCalibrator calibrator =
11675                 new KnownPositionAccelerometerCalibrator(ecefPosition,
11676                         true, bx, by, bz);
11677 
11678         // check default values
11679         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11680         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11681         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11682         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11683         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11684         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11685         final Acceleration bx2 = new Acceleration(0.0,
11686                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11687         calibrator.getInitialBiasXAsAcceleration(bx2);
11688         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11689         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11690         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11691         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11692         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11693         final Acceleration by2 = new Acceleration(0.0,
11694                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11695         calibrator.getInitialBiasYAsAcceleration(by2);
11696         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11697         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11698         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11699         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11700         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11701         final Acceleration bz2 = new Acceleration(0.0,
11702                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11703         calibrator.getInitialBiasZAsAcceleration(bz2);
11704         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11705         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11706         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11707         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11708         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11709         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11710         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11711         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11712         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11713         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11714         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11715         final double[] bias1 = calibrator.getInitialBias();
11716         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11717         final double[] bias2 = new double[3];
11718         calibrator.getInitialBias(bias2);
11719         assertArrayEquals(bias1, bias2, 0.0);
11720         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11721         assertEquals(b1, ba);
11722         final Matrix b2 = new Matrix(3, 1);
11723         calibrator.getInitialBiasAsMatrix(b2);
11724         assertEquals(b1, b2);
11725         final Matrix ma1 = calibrator.getInitialMa();
11726         assertEquals(ma1, new Matrix(3, 3));
11727         final Matrix ma2 = new Matrix(3, 3);
11728         calibrator.getInitialMa(ma2);
11729         assertEquals(ma1, ma2);
11730         assertSame(calibrator.getEcefPosition(), ecefPosition);
11731         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11732         final NEDPosition nedPosition1 = new NEDPosition();
11733         assertTrue(calibrator.getNedPosition(nedPosition1));
11734         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11735         assertNull(calibrator.getMeasurements());
11736         assertTrue(calibrator.isCommonAxisUsed());
11737         assertNull(calibrator.getListener());
11738         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11739         assertFalse(calibrator.isReady());
11740         assertFalse(calibrator.isRunning());
11741         assertNull(calibrator.getEstimatedBiases());
11742         assertFalse(calibrator.getEstimatedBiases(null));
11743         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11744         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11745         assertNull(calibrator.getEstimatedBiasFx());
11746         assertNull(calibrator.getEstimatedBiasFy());
11747         assertNull(calibrator.getEstimatedBiasFz());
11748         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11749         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11750         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11751         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11752         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11753         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11754         assertNull(calibrator.getEstimatedMa());
11755         assertNull(calibrator.getEstimatedSx());
11756         assertNull(calibrator.getEstimatedSy());
11757         assertNull(calibrator.getEstimatedSz());
11758         assertNull(calibrator.getEstimatedMxy());
11759         assertNull(calibrator.getEstimatedMxz());
11760         assertNull(calibrator.getEstimatedMyx());
11761         assertNull(calibrator.getEstimatedMyz());
11762         assertNull(calibrator.getEstimatedMzx());
11763         assertNull(calibrator.getEstimatedMzy());
11764         assertNull(calibrator.getEstimatedCovariance());
11765         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11766         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11767                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11768         assertNotNull(calibrator.getGroundTruthGravityNorm());
11769         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11770         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11771         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11772                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11773         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11774         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11775         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11776     }
11777 
11778     @Test
11779     public void testConstructor100() throws WrongSizeException {
11780         final Matrix ba = generateBa();
11781         final double biasX = ba.getElementAtIndex(0);
11782         final double biasY = ba.getElementAtIndex(1);
11783         final double biasZ = ba.getElementAtIndex(2);
11784 
11785         final Acceleration bx = new Acceleration(biasX,
11786                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11787         final Acceleration by = new Acceleration(biasY,
11788                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11789         final Acceleration bz = new Acceleration(biasZ,
11790                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11791 
11792         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11793         final double latitude = Math.toRadians(
11794                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11795         final double longitude = Math.toRadians(
11796                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11797         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11798         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11799         final NEDVelocity nedVelocity = new NEDVelocity();
11800         final ECEFPosition ecefPosition = new ECEFPosition();
11801         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11802         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11803                 ecefPosition, ecefVelocity);
11804 
11805         final KnownPositionAccelerometerCalibrator calibrator =
11806                 new KnownPositionAccelerometerCalibrator(ecefPosition,
11807                         true, bx, by, bz, this);
11808 
11809         // check default values
11810         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11811         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11812         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11813         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11814         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11815         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11816         final Acceleration bx2 = new Acceleration(0.0,
11817                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11818         calibrator.getInitialBiasXAsAcceleration(bx2);
11819         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11820         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11821         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11822         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11823         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11824         final Acceleration by2 = new Acceleration(0.0,
11825                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11826         calibrator.getInitialBiasYAsAcceleration(by2);
11827         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11828         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11829         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11830         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11831         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11832         final Acceleration bz2 = new Acceleration(0.0,
11833                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11834         calibrator.getInitialBiasZAsAcceleration(bz2);
11835         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11836         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11837         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11838         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11839         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11840         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11841         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11842         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11843         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11844         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11845         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11846         final double[] bias1 = calibrator.getInitialBias();
11847         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11848         final double[] bias2 = new double[3];
11849         calibrator.getInitialBias(bias2);
11850         assertArrayEquals(bias1, bias2, 0.0);
11851         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11852         assertEquals(b1, ba);
11853         final Matrix b2 = new Matrix(3, 1);
11854         calibrator.getInitialBiasAsMatrix(b2);
11855         assertEquals(b1, b2);
11856         final Matrix ma1 = calibrator.getInitialMa();
11857         assertEquals(ma1, new Matrix(3, 3));
11858         final Matrix ma2 = new Matrix(3, 3);
11859         calibrator.getInitialMa(ma2);
11860         assertEquals(ma1, ma2);
11861         assertSame(calibrator.getEcefPosition(), ecefPosition);
11862         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11863         final NEDPosition nedPosition1 = new NEDPosition();
11864         assertTrue(calibrator.getNedPosition(nedPosition1));
11865         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11866         assertNull(calibrator.getMeasurements());
11867         assertTrue(calibrator.isCommonAxisUsed());
11868         assertSame(calibrator.getListener(), this);
11869         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11870         assertFalse(calibrator.isReady());
11871         assertFalse(calibrator.isRunning());
11872         assertNull(calibrator.getEstimatedBiases());
11873         assertFalse(calibrator.getEstimatedBiases(null));
11874         assertNull(calibrator.getEstimatedBiasesAsMatrix());
11875         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11876         assertNull(calibrator.getEstimatedBiasFx());
11877         assertNull(calibrator.getEstimatedBiasFy());
11878         assertNull(calibrator.getEstimatedBiasFz());
11879         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11880         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11881         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11882         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11883         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11884         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11885         assertNull(calibrator.getEstimatedMa());
11886         assertNull(calibrator.getEstimatedSx());
11887         assertNull(calibrator.getEstimatedSy());
11888         assertNull(calibrator.getEstimatedSz());
11889         assertNull(calibrator.getEstimatedMxy());
11890         assertNull(calibrator.getEstimatedMxz());
11891         assertNull(calibrator.getEstimatedMyx());
11892         assertNull(calibrator.getEstimatedMyz());
11893         assertNull(calibrator.getEstimatedMzx());
11894         assertNull(calibrator.getEstimatedMzy());
11895         assertNull(calibrator.getEstimatedCovariance());
11896         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11897         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11898                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11899         assertNotNull(calibrator.getGroundTruthGravityNorm());
11900         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11901         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11902         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11903                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11904         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11905         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11906         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11907     }
11908 
11909     @Test
11910     public void testConstructor101() throws WrongSizeException {
11911         final Collection<StandardDeviationBodyKinematics> measurements =
11912                 Collections.emptyList();
11913 
11914         final Matrix ba = generateBa();
11915         final double biasX = ba.getElementAtIndex(0);
11916         final double biasY = ba.getElementAtIndex(1);
11917         final double biasZ = ba.getElementAtIndex(2);
11918 
11919         final Acceleration bx = new Acceleration(biasX,
11920                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11921         final Acceleration by = new Acceleration(biasY,
11922                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11923         final Acceleration bz = new Acceleration(biasZ,
11924                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11925 
11926         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11927         final double latitude = Math.toRadians(
11928                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11929         final double longitude = Math.toRadians(
11930                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11931         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11932         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11933         final NEDVelocity nedVelocity = new NEDVelocity();
11934         final ECEFPosition ecefPosition = new ECEFPosition();
11935         final ECEFVelocity ecefVelocity = new ECEFVelocity();
11936         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11937                 ecefPosition, ecefVelocity);
11938 
11939         final KnownPositionAccelerometerCalibrator calibrator =
11940                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11941                         true, bx, by, bz);
11942 
11943         // check default values
11944         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11945         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11946         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11947         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11948         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11949         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11950         final Acceleration bx2 = new Acceleration(0.0,
11951                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11952         calibrator.getInitialBiasXAsAcceleration(bx2);
11953         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11954         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11955         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11956         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11957         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11958         final Acceleration by2 = new Acceleration(0.0,
11959                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11960         calibrator.getInitialBiasYAsAcceleration(by2);
11961         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11962         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11963         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11964         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11965         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11966         final Acceleration bz2 = new Acceleration(0.0,
11967                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11968         calibrator.getInitialBiasZAsAcceleration(bz2);
11969         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11970         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11971         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11972         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11973         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11974         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11975         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11976         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11977         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11978         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11979         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11980         final double[] bias1 = calibrator.getInitialBias();
11981         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11982         final double[] bias2 = new double[3];
11983         calibrator.getInitialBias(bias2);
11984         assertArrayEquals(bias1, bias2, 0.0);
11985         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11986         assertEquals(b1, ba);
11987         final Matrix b2 = new Matrix(3, 1);
11988         calibrator.getInitialBiasAsMatrix(b2);
11989         assertEquals(b1, b2);
11990         final Matrix ma1 = calibrator.getInitialMa();
11991         assertEquals(ma1, new Matrix(3, 3));
11992         final Matrix ma2 = new Matrix(3, 3);
11993         calibrator.getInitialMa(ma2);
11994         assertEquals(ma1, ma2);
11995         assertSame(calibrator.getEcefPosition(), ecefPosition);
11996         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11997         final NEDPosition nedPosition1 = new NEDPosition();
11998         assertTrue(calibrator.getNedPosition(nedPosition1));
11999         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12000         assertSame(calibrator.getMeasurements(), measurements);
12001         assertTrue(calibrator.isCommonAxisUsed());
12002         assertNull(calibrator.getListener());
12003         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12004         assertFalse(calibrator.isReady());
12005         assertFalse(calibrator.isRunning());
12006         assertNull(calibrator.getEstimatedBiases());
12007         assertFalse(calibrator.getEstimatedBiases(null));
12008         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12009         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12010         assertNull(calibrator.getEstimatedBiasFx());
12011         assertNull(calibrator.getEstimatedBiasFy());
12012         assertNull(calibrator.getEstimatedBiasFz());
12013         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12014         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12015         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12016         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12017         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12018         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12019         assertNull(calibrator.getEstimatedMa());
12020         assertNull(calibrator.getEstimatedSx());
12021         assertNull(calibrator.getEstimatedSy());
12022         assertNull(calibrator.getEstimatedSz());
12023         assertNull(calibrator.getEstimatedMxy());
12024         assertNull(calibrator.getEstimatedMxz());
12025         assertNull(calibrator.getEstimatedMyx());
12026         assertNull(calibrator.getEstimatedMyz());
12027         assertNull(calibrator.getEstimatedMzx());
12028         assertNull(calibrator.getEstimatedMzy());
12029         assertNull(calibrator.getEstimatedCovariance());
12030         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12031         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12032                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12033         assertNotNull(calibrator.getGroundTruthGravityNorm());
12034         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12035         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12036         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12037                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12038         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12039         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12040         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12041     }
12042 
12043     @Test
12044     public void testConstructor102() throws WrongSizeException {
12045         final Collection<StandardDeviationBodyKinematics> measurements =
12046                 Collections.emptyList();
12047 
12048         final Matrix ba = generateBa();
12049         final double biasX = ba.getElementAtIndex(0);
12050         final double biasY = ba.getElementAtIndex(1);
12051         final double biasZ = ba.getElementAtIndex(2);
12052 
12053         final Acceleration bx = new Acceleration(biasX,
12054                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12055         final Acceleration by = new Acceleration(biasY,
12056                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12057         final Acceleration bz = new Acceleration(biasZ,
12058                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12059 
12060         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12061         final double latitude = Math.toRadians(
12062                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12063         final double longitude = Math.toRadians(
12064                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12065         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12066         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12067         final NEDVelocity nedVelocity = new NEDVelocity();
12068         final ECEFPosition ecefPosition = new ECEFPosition();
12069         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12070         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12071                 ecefPosition, ecefVelocity);
12072 
12073         final KnownPositionAccelerometerCalibrator calibrator =
12074                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12075                         true, bx, by, bz, this);
12076 
12077         // check default values
12078         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12079         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12080         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12081         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12082         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12083         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12084         final Acceleration bx2 = new Acceleration(0.0,
12085                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12086         calibrator.getInitialBiasXAsAcceleration(bx2);
12087         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12088         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12089         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12090         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12091         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12092         final Acceleration by2 = new Acceleration(0.0,
12093                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12094         calibrator.getInitialBiasYAsAcceleration(by2);
12095         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12096         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12097         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12098         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12099         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12100         final Acceleration bz2 = new Acceleration(0.0,
12101                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12102         calibrator.getInitialBiasZAsAcceleration(bz2);
12103         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12104         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12105         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
12106         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
12107         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
12108         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12109         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12110         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12111         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12112         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12113         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12114         final double[] bias1 = calibrator.getInitialBias();
12115         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12116         final double[] bias2 = new double[3];
12117         calibrator.getInitialBias(bias2);
12118         assertArrayEquals(bias1, bias2, 0.0);
12119         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12120         assertEquals(b1, ba);
12121         final Matrix b2 = new Matrix(3, 1);
12122         calibrator.getInitialBiasAsMatrix(b2);
12123         assertEquals(b1, b2);
12124         final Matrix ma1 = calibrator.getInitialMa();
12125         assertEquals(ma1, new Matrix(3, 3));
12126         final Matrix ma2 = new Matrix(3, 3);
12127         calibrator.getInitialMa(ma2);
12128         assertEquals(ma1, ma2);
12129         assertSame(calibrator.getEcefPosition(), ecefPosition);
12130         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12131         final NEDPosition nedPosition1 = new NEDPosition();
12132         assertTrue(calibrator.getNedPosition(nedPosition1));
12133         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12134         assertSame(calibrator.getMeasurements(), measurements);
12135         assertTrue(calibrator.isCommonAxisUsed());
12136         assertSame(calibrator.getListener(), this);
12137         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12138         assertFalse(calibrator.isReady());
12139         assertFalse(calibrator.isRunning());
12140         assertNull(calibrator.getEstimatedBiases());
12141         assertFalse(calibrator.getEstimatedBiases(null));
12142         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12143         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12144         assertNull(calibrator.getEstimatedBiasFx());
12145         assertNull(calibrator.getEstimatedBiasFy());
12146         assertNull(calibrator.getEstimatedBiasFz());
12147         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12148         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12149         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12150         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12151         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12152         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12153         assertNull(calibrator.getEstimatedMa());
12154         assertNull(calibrator.getEstimatedSx());
12155         assertNull(calibrator.getEstimatedSy());
12156         assertNull(calibrator.getEstimatedSz());
12157         assertNull(calibrator.getEstimatedMxy());
12158         assertNull(calibrator.getEstimatedMxz());
12159         assertNull(calibrator.getEstimatedMyx());
12160         assertNull(calibrator.getEstimatedMyz());
12161         assertNull(calibrator.getEstimatedMzx());
12162         assertNull(calibrator.getEstimatedMzy());
12163         assertNull(calibrator.getEstimatedCovariance());
12164         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12165         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12166                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12167         assertNotNull(calibrator.getGroundTruthGravityNorm());
12168         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12169         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12170         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12171                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12172         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12173         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12174         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12175     }
12176 
12177     @Test
12178     public void testConstructor103() throws WrongSizeException {
12179         final Matrix ba = generateBa();
12180         final double biasX = ba.getElementAtIndex(0);
12181         final double biasY = ba.getElementAtIndex(1);
12182         final double biasZ = ba.getElementAtIndex(2);
12183 
12184         final Matrix ma = generateMaCommonAxis();
12185         final double sx = ma.getElementAt(0, 0);
12186         final double sy = ma.getElementAt(1, 1);
12187         final double sz = ma.getElementAt(2, 2);
12188 
12189         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12190         final double latitude = Math.toRadians(
12191                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12192         final double longitude = Math.toRadians(
12193                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12194         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12195         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12196         final NEDVelocity nedVelocity = new NEDVelocity();
12197         final ECEFPosition ecefPosition = new ECEFPosition();
12198         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12199         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12200                 ecefPosition, ecefVelocity);
12201 
12202         final KnownPositionAccelerometerCalibrator calibrator =
12203                 new KnownPositionAccelerometerCalibrator(ecefPosition,
12204                         biasX, biasY, biasZ, sx, sy, sz);
12205 
12206         // check default values
12207         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12208         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12209         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12210         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12211         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12212         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12213         final Acceleration bx2 = new Acceleration(0.0,
12214                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12215         calibrator.getInitialBiasXAsAcceleration(bx2);
12216         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12217         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12218         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12219         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12220         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12221         final Acceleration by2 = new Acceleration(0.0,
12222                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12223         calibrator.getInitialBiasYAsAcceleration(by2);
12224         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12225         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12226         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12227         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12228         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12229         final Acceleration bz2 = new Acceleration(0.0,
12230                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12231         calibrator.getInitialBiasZAsAcceleration(bz2);
12232         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12233         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12234         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12235         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12236         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12237         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12238         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12239         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12240         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12241         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12242         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12243         final double[] bias1 = calibrator.getInitialBias();
12244         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12245         final double[] bias2 = new double[3];
12246         calibrator.getInitialBias(bias2);
12247         assertArrayEquals(bias1, bias2, 0.0);
12248         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12249         assertEquals(b1, ba);
12250         final Matrix b2 = new Matrix(3, 1);
12251         calibrator.getInitialBiasAsMatrix(b2);
12252         assertEquals(b1, b2);
12253         final Matrix ma1 = calibrator.getInitialMa();
12254         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12255         final Matrix ma2 = new Matrix(3, 3);
12256         calibrator.getInitialMa(ma2);
12257         assertEquals(ma1, ma2);
12258         assertSame(calibrator.getEcefPosition(), ecefPosition);
12259         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12260         final NEDPosition nedPosition1 = new NEDPosition();
12261         assertTrue(calibrator.getNedPosition(nedPosition1));
12262         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12263         assertNull(calibrator.getMeasurements());
12264         assertFalse(calibrator.isCommonAxisUsed());
12265         assertNull(calibrator.getListener());
12266         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12267         assertFalse(calibrator.isReady());
12268         assertFalse(calibrator.isRunning());
12269         assertNull(calibrator.getEstimatedBiases());
12270         assertFalse(calibrator.getEstimatedBiases(null));
12271         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12272         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12273         assertNull(calibrator.getEstimatedBiasFx());
12274         assertNull(calibrator.getEstimatedBiasFy());
12275         assertNull(calibrator.getEstimatedBiasFz());
12276         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12277         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12278         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12279         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12280         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12281         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12282         assertNull(calibrator.getEstimatedMa());
12283         assertNull(calibrator.getEstimatedSx());
12284         assertNull(calibrator.getEstimatedSy());
12285         assertNull(calibrator.getEstimatedSz());
12286         assertNull(calibrator.getEstimatedMxy());
12287         assertNull(calibrator.getEstimatedMxz());
12288         assertNull(calibrator.getEstimatedMyx());
12289         assertNull(calibrator.getEstimatedMyz());
12290         assertNull(calibrator.getEstimatedMzx());
12291         assertNull(calibrator.getEstimatedMzy());
12292         assertNull(calibrator.getEstimatedCovariance());
12293         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12294         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12295                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12296         assertNotNull(calibrator.getGroundTruthGravityNorm());
12297         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12298         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12299         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12300                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12301         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12302         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12303         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12304     }
12305 
12306     @Test
12307     public void testConstructor104() throws WrongSizeException {
12308         final Collection<StandardDeviationBodyKinematics> measurements =
12309                 Collections.emptyList();
12310 
12311         final Matrix ba = generateBa();
12312         final double biasX = ba.getElementAtIndex(0);
12313         final double biasY = ba.getElementAtIndex(1);
12314         final double biasZ = ba.getElementAtIndex(2);
12315 
12316         final Matrix ma = generateMaCommonAxis();
12317         final double sx = ma.getElementAt(0, 0);
12318         final double sy = ma.getElementAt(1, 1);
12319         final double sz = ma.getElementAt(2, 2);
12320 
12321         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12322         final double latitude = Math.toRadians(
12323                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12324         final double longitude = Math.toRadians(
12325                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12326         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12327         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12328         final NEDVelocity nedVelocity = new NEDVelocity();
12329         final ECEFPosition ecefPosition = new ECEFPosition();
12330         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12331         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12332                 ecefPosition, ecefVelocity);
12333 
12334         final KnownPositionAccelerometerCalibrator calibrator =
12335                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12336                         biasX, biasY, biasZ, sx, sy, sz);
12337 
12338         // check default values
12339         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12340         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12341         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12342         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12343         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12344         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12345         final Acceleration bx2 = new Acceleration(0.0,
12346                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12347         calibrator.getInitialBiasXAsAcceleration(bx2);
12348         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12349         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12350         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12351         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12352         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12353         final Acceleration by2 = new Acceleration(0.0,
12354                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12355         calibrator.getInitialBiasYAsAcceleration(by2);
12356         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12357         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12358         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12359         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12360         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12361         final Acceleration bz2 = new Acceleration(0.0,
12362                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12363         calibrator.getInitialBiasZAsAcceleration(bz2);
12364         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12365         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12366         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12367         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12368         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12369         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12370         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12371         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12372         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12373         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12374         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12375         final double[] bias1 = calibrator.getInitialBias();
12376         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12377         final double[] bias2 = new double[3];
12378         calibrator.getInitialBias(bias2);
12379         assertArrayEquals(bias1, bias2, 0.0);
12380         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12381         assertEquals(b1, ba);
12382         final Matrix b2 = new Matrix(3, 1);
12383         calibrator.getInitialBiasAsMatrix(b2);
12384         assertEquals(b1, b2);
12385         final Matrix ma1 = calibrator.getInitialMa();
12386         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12387         final Matrix ma2 = new Matrix(3, 3);
12388         calibrator.getInitialMa(ma2);
12389         assertEquals(ma1, ma2);
12390         assertSame(calibrator.getEcefPosition(), ecefPosition);
12391         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12392         final NEDPosition nedPosition1 = new NEDPosition();
12393         assertTrue(calibrator.getNedPosition(nedPosition1));
12394         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12395         assertSame(calibrator.getMeasurements(), measurements);
12396         assertFalse(calibrator.isCommonAxisUsed());
12397         assertNull(calibrator.getListener());
12398         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12399         assertFalse(calibrator.isReady());
12400         assertFalse(calibrator.isRunning());
12401         assertNull(calibrator.getEstimatedBiases());
12402         assertFalse(calibrator.getEstimatedBiases(null));
12403         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12404         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12405         assertNull(calibrator.getEstimatedBiasFx());
12406         assertNull(calibrator.getEstimatedBiasFy());
12407         assertNull(calibrator.getEstimatedBiasFz());
12408         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12409         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12410         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12411         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12412         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12413         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12414         assertNull(calibrator.getEstimatedMa());
12415         assertNull(calibrator.getEstimatedSx());
12416         assertNull(calibrator.getEstimatedSy());
12417         assertNull(calibrator.getEstimatedSz());
12418         assertNull(calibrator.getEstimatedMxy());
12419         assertNull(calibrator.getEstimatedMxz());
12420         assertNull(calibrator.getEstimatedMyx());
12421         assertNull(calibrator.getEstimatedMyz());
12422         assertNull(calibrator.getEstimatedMzx());
12423         assertNull(calibrator.getEstimatedMzy());
12424         assertNull(calibrator.getEstimatedCovariance());
12425         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12426         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12427                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12428         assertNotNull(calibrator.getGroundTruthGravityNorm());
12429         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12430         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12431         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12432                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12433         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12434         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12435         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12436     }
12437 
12438     @Test
12439     public void testConstructor105() throws WrongSizeException {
12440         final Collection<StandardDeviationBodyKinematics> measurements =
12441                 Collections.emptyList();
12442 
12443         final Matrix ba = generateBa();
12444         final double biasX = ba.getElementAtIndex(0);
12445         final double biasY = ba.getElementAtIndex(1);
12446         final double biasZ = ba.getElementAtIndex(2);
12447 
12448         final Matrix ma = generateMaCommonAxis();
12449         final double sx = ma.getElementAt(0, 0);
12450         final double sy = ma.getElementAt(1, 1);
12451         final double sz = ma.getElementAt(2, 2);
12452 
12453         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12454         final double latitude = Math.toRadians(
12455                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12456         final double longitude = Math.toRadians(
12457                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12458         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12459         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12460         final NEDVelocity nedVelocity = new NEDVelocity();
12461         final ECEFPosition ecefPosition = new ECEFPosition();
12462         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12463         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12464                 ecefPosition, ecefVelocity);
12465 
12466         final KnownPositionAccelerometerCalibrator calibrator =
12467                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12468                         biasX, biasY, biasZ, sx, sy, sz, this);
12469 
12470         // check default values
12471         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12472         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12473         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12474         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12475         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12476         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12477         final Acceleration bx2 = new Acceleration(0.0,
12478                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12479         calibrator.getInitialBiasXAsAcceleration(bx2);
12480         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12481         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12482         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12483         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12484         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12485         final Acceleration by2 = new Acceleration(0.0,
12486                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12487         calibrator.getInitialBiasYAsAcceleration(by2);
12488         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12489         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12490         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12491         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12492         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12493         final Acceleration bz2 = new Acceleration(0.0,
12494                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12495         calibrator.getInitialBiasZAsAcceleration(bz2);
12496         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12497         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12498         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12499         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12500         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12501         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12502         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12503         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12504         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12505         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12506         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12507         final double[] bias1 = calibrator.getInitialBias();
12508         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12509         final double[] bias2 = new double[3];
12510         calibrator.getInitialBias(bias2);
12511         assertArrayEquals(bias1, bias2, 0.0);
12512         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12513         assertEquals(b1, ba);
12514         final Matrix b2 = new Matrix(3, 1);
12515         calibrator.getInitialBiasAsMatrix(b2);
12516         assertEquals(b1, b2);
12517         final Matrix ma1 = calibrator.getInitialMa();
12518         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12519         final Matrix ma2 = new Matrix(3, 3);
12520         calibrator.getInitialMa(ma2);
12521         assertEquals(ma1, ma2);
12522         assertSame(calibrator.getEcefPosition(), ecefPosition);
12523         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12524         final NEDPosition nedPosition1 = new NEDPosition();
12525         assertTrue(calibrator.getNedPosition(nedPosition1));
12526         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12527         assertSame(calibrator.getMeasurements(), measurements);
12528         assertFalse(calibrator.isCommonAxisUsed());
12529         assertSame(calibrator.getListener(), this);
12530         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12531         assertFalse(calibrator.isReady());
12532         assertFalse(calibrator.isRunning());
12533         assertNull(calibrator.getEstimatedBiases());
12534         assertFalse(calibrator.getEstimatedBiases(null));
12535         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12536         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12537         assertNull(calibrator.getEstimatedBiasFx());
12538         assertNull(calibrator.getEstimatedBiasFy());
12539         assertNull(calibrator.getEstimatedBiasFz());
12540         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12541         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12542         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12543         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12544         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12545         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12546         assertNull(calibrator.getEstimatedMa());
12547         assertNull(calibrator.getEstimatedSx());
12548         assertNull(calibrator.getEstimatedSy());
12549         assertNull(calibrator.getEstimatedSz());
12550         assertNull(calibrator.getEstimatedMxy());
12551         assertNull(calibrator.getEstimatedMxz());
12552         assertNull(calibrator.getEstimatedMyx());
12553         assertNull(calibrator.getEstimatedMyz());
12554         assertNull(calibrator.getEstimatedMzx());
12555         assertNull(calibrator.getEstimatedMzy());
12556         assertNull(calibrator.getEstimatedCovariance());
12557         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12558         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12559                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12560         assertNotNull(calibrator.getGroundTruthGravityNorm());
12561         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12562         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12563         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12564                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12565         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12566         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12567         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12568     }
12569 
12570     @Test
12571     public void testConstructor106() throws WrongSizeException {
12572         final Matrix ba = generateBa();
12573         final double biasX = ba.getElementAtIndex(0);
12574         final double biasY = ba.getElementAtIndex(1);
12575         final double biasZ = ba.getElementAtIndex(2);
12576 
12577         final Matrix ma = generateMaCommonAxis();
12578         final double sx = ma.getElementAt(0, 0);
12579         final double sy = ma.getElementAt(1, 1);
12580         final double sz = ma.getElementAt(2, 2);
12581 
12582         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12583         final double latitude = Math.toRadians(
12584                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12585         final double longitude = Math.toRadians(
12586                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12587         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12588         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12589         final NEDVelocity nedVelocity = new NEDVelocity();
12590         final ECEFPosition ecefPosition = new ECEFPosition();
12591         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12592         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12593                 ecefPosition, ecefVelocity);
12594 
12595         final KnownPositionAccelerometerCalibrator calibrator =
12596                 new KnownPositionAccelerometerCalibrator(ecefPosition,
12597                         true, biasX, biasY, biasZ, sx, sy, sz);
12598 
12599         // check default values
12600         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12601         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12602         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12603         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12604         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12605         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12606         final Acceleration bx2 = new Acceleration(0.0,
12607                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12608         calibrator.getInitialBiasXAsAcceleration(bx2);
12609         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12610         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12611         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12612         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12613         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12614         final Acceleration by2 = new Acceleration(0.0,
12615                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12616         calibrator.getInitialBiasYAsAcceleration(by2);
12617         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12618         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12619         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12620         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12621         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12622         final Acceleration bz2 = new Acceleration(0.0,
12623                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12624         calibrator.getInitialBiasZAsAcceleration(bz2);
12625         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12626         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12627         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12628         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12629         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12630         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12631         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12632         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12633         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12634         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12635         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12636         final double[] bias1 = calibrator.getInitialBias();
12637         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12638         final double[] bias2 = new double[3];
12639         calibrator.getInitialBias(bias2);
12640         assertArrayEquals(bias1, bias2, 0.0);
12641         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12642         assertEquals(b1, ba);
12643         final Matrix b2 = new Matrix(3, 1);
12644         calibrator.getInitialBiasAsMatrix(b2);
12645         assertEquals(b1, b2);
12646         final Matrix ma1 = calibrator.getInitialMa();
12647         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12648         final Matrix ma2 = new Matrix(3, 3);
12649         calibrator.getInitialMa(ma2);
12650         assertEquals(ma1, ma2);
12651         assertSame(calibrator.getEcefPosition(), ecefPosition);
12652         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12653         final NEDPosition nedPosition1 = new NEDPosition();
12654         assertTrue(calibrator.getNedPosition(nedPosition1));
12655         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12656         assertNull(calibrator.getMeasurements());
12657         assertTrue(calibrator.isCommonAxisUsed());
12658         assertNull(calibrator.getListener());
12659         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12660         assertFalse(calibrator.isReady());
12661         assertFalse(calibrator.isRunning());
12662         assertNull(calibrator.getEstimatedBiases());
12663         assertFalse(calibrator.getEstimatedBiases(null));
12664         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12665         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12666         assertNull(calibrator.getEstimatedBiasFx());
12667         assertNull(calibrator.getEstimatedBiasFy());
12668         assertNull(calibrator.getEstimatedBiasFz());
12669         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12670         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12671         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12672         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12673         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12674         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12675         assertNull(calibrator.getEstimatedMa());
12676         assertNull(calibrator.getEstimatedSx());
12677         assertNull(calibrator.getEstimatedSy());
12678         assertNull(calibrator.getEstimatedSz());
12679         assertNull(calibrator.getEstimatedMxy());
12680         assertNull(calibrator.getEstimatedMxz());
12681         assertNull(calibrator.getEstimatedMyx());
12682         assertNull(calibrator.getEstimatedMyz());
12683         assertNull(calibrator.getEstimatedMzx());
12684         assertNull(calibrator.getEstimatedMzy());
12685         assertNull(calibrator.getEstimatedCovariance());
12686         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12687         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12688                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12689         assertNotNull(calibrator.getGroundTruthGravityNorm());
12690         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12691         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12692         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12693                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12694         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12695         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12696         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12697     }
12698 
12699     @Test
12700     public void testConstructor107() throws WrongSizeException {
12701         final Matrix ba = generateBa();
12702         final double biasX = ba.getElementAtIndex(0);
12703         final double biasY = ba.getElementAtIndex(1);
12704         final double biasZ = ba.getElementAtIndex(2);
12705 
12706         final Matrix ma = generateMaCommonAxis();
12707         final double sx = ma.getElementAt(0, 0);
12708         final double sy = ma.getElementAt(1, 1);
12709         final double sz = ma.getElementAt(2, 2);
12710 
12711         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12712         final double latitude = Math.toRadians(
12713                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12714         final double longitude = Math.toRadians(
12715                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12716         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12717         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12718         final NEDVelocity nedVelocity = new NEDVelocity();
12719         final ECEFPosition ecefPosition = new ECEFPosition();
12720         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12721         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12722                 ecefPosition, ecefVelocity);
12723 
12724         final KnownPositionAccelerometerCalibrator calibrator =
12725                 new KnownPositionAccelerometerCalibrator(ecefPosition,
12726                         true, biasX, biasY, biasZ, sx, sy, sz,
12727                         this);
12728 
12729         // check default values
12730         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12731         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12732         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12733         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12734         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12735         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12736         final Acceleration bx2 = new Acceleration(0.0,
12737                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12738         calibrator.getInitialBiasXAsAcceleration(bx2);
12739         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12740         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12741         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12742         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12743         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12744         final Acceleration by2 = new Acceleration(0.0,
12745                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12746         calibrator.getInitialBiasYAsAcceleration(by2);
12747         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12748         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12749         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12750         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12751         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12752         final Acceleration bz2 = new Acceleration(0.0,
12753                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12754         calibrator.getInitialBiasZAsAcceleration(bz2);
12755         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12756         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12757         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12758         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12759         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12760         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12761         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12762         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12763         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12764         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12765         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12766         final double[] bias1 = calibrator.getInitialBias();
12767         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12768         final double[] bias2 = new double[3];
12769         calibrator.getInitialBias(bias2);
12770         assertArrayEquals(bias1, bias2, 0.0);
12771         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12772         assertEquals(b1, ba);
12773         final Matrix b2 = new Matrix(3, 1);
12774         calibrator.getInitialBiasAsMatrix(b2);
12775         assertEquals(b1, b2);
12776         final Matrix ma1 = calibrator.getInitialMa();
12777         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12778         final Matrix ma2 = new Matrix(3, 3);
12779         calibrator.getInitialMa(ma2);
12780         assertEquals(ma1, ma2);
12781         assertSame(calibrator.getEcefPosition(), ecefPosition);
12782         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12783         final NEDPosition nedPosition1 = new NEDPosition();
12784         assertTrue(calibrator.getNedPosition(nedPosition1));
12785         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12786         assertNull(calibrator.getMeasurements());
12787         assertTrue(calibrator.isCommonAxisUsed());
12788         assertSame(calibrator.getListener(), this);
12789         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12790         assertFalse(calibrator.isReady());
12791         assertFalse(calibrator.isRunning());
12792         assertNull(calibrator.getEstimatedBiases());
12793         assertFalse(calibrator.getEstimatedBiases(null));
12794         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12795         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12796         assertNull(calibrator.getEstimatedBiasFx());
12797         assertNull(calibrator.getEstimatedBiasFy());
12798         assertNull(calibrator.getEstimatedBiasFz());
12799         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12800         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12801         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12802         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12803         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12804         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12805         assertNull(calibrator.getEstimatedMa());
12806         assertNull(calibrator.getEstimatedSx());
12807         assertNull(calibrator.getEstimatedSy());
12808         assertNull(calibrator.getEstimatedSz());
12809         assertNull(calibrator.getEstimatedMxy());
12810         assertNull(calibrator.getEstimatedMxz());
12811         assertNull(calibrator.getEstimatedMyx());
12812         assertNull(calibrator.getEstimatedMyz());
12813         assertNull(calibrator.getEstimatedMzx());
12814         assertNull(calibrator.getEstimatedMzy());
12815         assertNull(calibrator.getEstimatedCovariance());
12816         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12817         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12818                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12819         assertNotNull(calibrator.getGroundTruthGravityNorm());
12820         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12821         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12822         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12823                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12824         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12825         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12826         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12827     }
12828 
12829     @Test
12830     public void testConstructor108() throws WrongSizeException {
12831         final Collection<StandardDeviationBodyKinematics> measurements =
12832                 Collections.emptyList();
12833 
12834         final Matrix ba = generateBa();
12835         final double biasX = ba.getElementAtIndex(0);
12836         final double biasY = ba.getElementAtIndex(1);
12837         final double biasZ = ba.getElementAtIndex(2);
12838 
12839         final Matrix ma = generateMaCommonAxis();
12840         final double sx = ma.getElementAt(0, 0);
12841         final double sy = ma.getElementAt(1, 1);
12842         final double sz = ma.getElementAt(2, 2);
12843 
12844         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12845         final double latitude = Math.toRadians(
12846                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12847         final double longitude = Math.toRadians(
12848                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12849         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12850         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12851         final NEDVelocity nedVelocity = new NEDVelocity();
12852         final ECEFPosition ecefPosition = new ECEFPosition();
12853         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12854         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12855                 ecefPosition, ecefVelocity);
12856 
12857         final KnownPositionAccelerometerCalibrator calibrator =
12858                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12859                         true, biasX, biasY, biasZ, sx, sy, sz);
12860 
12861         // check default values
12862         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12863         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12864         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12865         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12866         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12867         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12868         final Acceleration bx2 = new Acceleration(0.0,
12869                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12870         calibrator.getInitialBiasXAsAcceleration(bx2);
12871         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12872         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12873         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12874         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12875         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12876         final Acceleration by2 = new Acceleration(0.0,
12877                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12878         calibrator.getInitialBiasYAsAcceleration(by2);
12879         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12880         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12881         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12882         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12883         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12884         final Acceleration bz2 = new Acceleration(0.0,
12885                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12886         calibrator.getInitialBiasZAsAcceleration(bz2);
12887         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12888         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12889         assertEquals(calibrator.getInitialSx(), sx, 0.0);
12890         assertEquals(calibrator.getInitialSy(), sy, 0.0);
12891         assertEquals(calibrator.getInitialSz(), sz, 0.0);
12892         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12893         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12894         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12895         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12896         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12897         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12898         final double[] bias1 = calibrator.getInitialBias();
12899         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12900         final double[] bias2 = new double[3];
12901         calibrator.getInitialBias(bias2);
12902         assertArrayEquals(bias1, bias2, 0.0);
12903         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12904         assertEquals(b1, ba);
12905         final Matrix b2 = new Matrix(3, 1);
12906         calibrator.getInitialBiasAsMatrix(b2);
12907         assertEquals(b1, b2);
12908         final Matrix ma1 = calibrator.getInitialMa();
12909         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12910         final Matrix ma2 = new Matrix(3, 3);
12911         calibrator.getInitialMa(ma2);
12912         assertEquals(ma1, ma2);
12913         assertSame(calibrator.getEcefPosition(), ecefPosition);
12914         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12915         final NEDPosition nedPosition1 = new NEDPosition();
12916         assertTrue(calibrator.getNedPosition(nedPosition1));
12917         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12918         assertSame(calibrator.getMeasurements(), measurements);
12919         assertTrue(calibrator.isCommonAxisUsed());
12920         assertNull(calibrator.getListener());
12921         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12922         assertFalse(calibrator.isReady());
12923         assertFalse(calibrator.isRunning());
12924         assertNull(calibrator.getEstimatedBiases());
12925         assertFalse(calibrator.getEstimatedBiases(null));
12926         assertNull(calibrator.getEstimatedBiasesAsMatrix());
12927         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12928         assertNull(calibrator.getEstimatedBiasFx());
12929         assertNull(calibrator.getEstimatedBiasFy());
12930         assertNull(calibrator.getEstimatedBiasFz());
12931         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12932         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12933         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12934         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12935         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12936         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12937         assertNull(calibrator.getEstimatedMa());
12938         assertNull(calibrator.getEstimatedSx());
12939         assertNull(calibrator.getEstimatedSy());
12940         assertNull(calibrator.getEstimatedSz());
12941         assertNull(calibrator.getEstimatedMxy());
12942         assertNull(calibrator.getEstimatedMxz());
12943         assertNull(calibrator.getEstimatedMyx());
12944         assertNull(calibrator.getEstimatedMyz());
12945         assertNull(calibrator.getEstimatedMzx());
12946         assertNull(calibrator.getEstimatedMzy());
12947         assertNull(calibrator.getEstimatedCovariance());
12948         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12949         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12950                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12951         assertNotNull(calibrator.getGroundTruthGravityNorm());
12952         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12953         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12954         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12955                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12956         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12957         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12958         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12959     }
12960 
12961     @Test
12962     public void testConstructor109() throws WrongSizeException {
12963         final Collection<StandardDeviationBodyKinematics> measurements =
12964                 Collections.emptyList();
12965 
12966         final Matrix ba = generateBa();
12967         final double biasX = ba.getElementAtIndex(0);
12968         final double biasY = ba.getElementAtIndex(1);
12969         final double biasZ = ba.getElementAtIndex(2);
12970 
12971         final Matrix ma = generateMaCommonAxis();
12972         final double sx = ma.getElementAt(0, 0);
12973         final double sy = ma.getElementAt(1, 1);
12974         final double sz = ma.getElementAt(2, 2);
12975 
12976         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12977         final double latitude = Math.toRadians(
12978                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12979         final double longitude = Math.toRadians(
12980                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12981         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12982         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12983         final NEDVelocity nedVelocity = new NEDVelocity();
12984         final ECEFPosition ecefPosition = new ECEFPosition();
12985         final ECEFVelocity ecefVelocity = new ECEFVelocity();
12986         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12987                 ecefPosition, ecefVelocity);
12988 
12989         final KnownPositionAccelerometerCalibrator calibrator =
12990                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12991                         true, biasX, biasY, biasZ, sx, sy, sz,
12992                         this);
12993 
12994         // check default values
12995         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12996         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12997         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12998         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12999         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13000         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13001         final Acceleration bx2 = new Acceleration(0.0,
13002                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13003         calibrator.getInitialBiasXAsAcceleration(bx2);
13004         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13005         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13006         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13007         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13008         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13009         final Acceleration by2 = new Acceleration(0.0,
13010                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13011         calibrator.getInitialBiasYAsAcceleration(by2);
13012         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13013         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13014         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13015         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13016         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13017         final Acceleration bz2 = new Acceleration(0.0,
13018                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13019         calibrator.getInitialBiasZAsAcceleration(bz2);
13020         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13021         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13022         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13023         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13024         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13025         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13026         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13027         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13028         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13029         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13030         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13031         final double[] bias1 = calibrator.getInitialBias();
13032         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13033         final double[] bias2 = new double[3];
13034         calibrator.getInitialBias(bias2);
13035         assertArrayEquals(bias1, bias2, 0.0);
13036         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13037         assertEquals(b1, ba);
13038         final Matrix b2 = new Matrix(3, 1);
13039         calibrator.getInitialBiasAsMatrix(b2);
13040         assertEquals(b1, b2);
13041         final Matrix ma1 = calibrator.getInitialMa();
13042         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13043         final Matrix ma2 = new Matrix(3, 3);
13044         calibrator.getInitialMa(ma2);
13045         assertEquals(ma1, ma2);
13046         assertSame(calibrator.getEcefPosition(), ecefPosition);
13047         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13048         final NEDPosition nedPosition1 = new NEDPosition();
13049         assertTrue(calibrator.getNedPosition(nedPosition1));
13050         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13051         assertSame(calibrator.getMeasurements(), measurements);
13052         assertTrue(calibrator.isCommonAxisUsed());
13053         assertSame(calibrator.getListener(), this);
13054         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13055         assertFalse(calibrator.isReady());
13056         assertFalse(calibrator.isRunning());
13057         assertNull(calibrator.getEstimatedBiases());
13058         assertFalse(calibrator.getEstimatedBiases(null));
13059         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13060         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13061         assertNull(calibrator.getEstimatedBiasFx());
13062         assertNull(calibrator.getEstimatedBiasFy());
13063         assertNull(calibrator.getEstimatedBiasFz());
13064         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13065         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13066         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13067         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13068         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13069         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13070         assertNull(calibrator.getEstimatedMa());
13071         assertNull(calibrator.getEstimatedSx());
13072         assertNull(calibrator.getEstimatedSy());
13073         assertNull(calibrator.getEstimatedSz());
13074         assertNull(calibrator.getEstimatedMxy());
13075         assertNull(calibrator.getEstimatedMxz());
13076         assertNull(calibrator.getEstimatedMyx());
13077         assertNull(calibrator.getEstimatedMyz());
13078         assertNull(calibrator.getEstimatedMzx());
13079         assertNull(calibrator.getEstimatedMzy());
13080         assertNull(calibrator.getEstimatedCovariance());
13081         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13082         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13083                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13084         assertNotNull(calibrator.getGroundTruthGravityNorm());
13085         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13086         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13087         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13088                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13089         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13090         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13091         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13092     }
13093 
13094     @Test
13095     public void testConstructor110() throws WrongSizeException {
13096         final Matrix ba = generateBa();
13097         final double biasX = ba.getElementAtIndex(0);
13098         final double biasY = ba.getElementAtIndex(1);
13099         final double biasZ = ba.getElementAtIndex(2);
13100 
13101         final Acceleration bx = new Acceleration(biasX,
13102                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13103         final Acceleration by = new Acceleration(biasY,
13104                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13105         final Acceleration bz = new Acceleration(biasZ,
13106                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13107 
13108         final Matrix ma = generateMaCommonAxis();
13109         final double sx = ma.getElementAt(0, 0);
13110         final double sy = ma.getElementAt(1, 1);
13111         final double sz = ma.getElementAt(2, 2);
13112 
13113         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13114         final double latitude = Math.toRadians(
13115                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13116         final double longitude = Math.toRadians(
13117                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13118         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13119         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13120         final NEDVelocity nedVelocity = new NEDVelocity();
13121         final ECEFPosition ecefPosition = new ECEFPosition();
13122         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13123         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13124                 ecefPosition, ecefVelocity);
13125 
13126         final KnownPositionAccelerometerCalibrator calibrator =
13127                 new KnownPositionAccelerometerCalibrator(ecefPosition,
13128                         bx, by, bz, sx, sy, sz);
13129 
13130         // check default values
13131         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13132         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13133         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13134         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13135         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13136         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13137         final Acceleration bx2 = new Acceleration(0.0,
13138                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13139         calibrator.getInitialBiasXAsAcceleration(bx2);
13140         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13141         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13142         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13143         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13144         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13145         final Acceleration by2 = new Acceleration(0.0,
13146                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13147         calibrator.getInitialBiasYAsAcceleration(by2);
13148         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13149         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13150         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13151         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13152         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13153         final Acceleration bz2 = new Acceleration(0.0,
13154                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13155         calibrator.getInitialBiasZAsAcceleration(bz2);
13156         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13157         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13158         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13159         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13160         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13161         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13162         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13163         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13164         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13165         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13166         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13167         final double[] bias1 = calibrator.getInitialBias();
13168         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13169         final double[] bias2 = new double[3];
13170         calibrator.getInitialBias(bias2);
13171         assertArrayEquals(bias1, bias2, 0.0);
13172         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13173         assertEquals(b1, ba);
13174         final Matrix b2 = new Matrix(3, 1);
13175         calibrator.getInitialBiasAsMatrix(b2);
13176         assertEquals(b1, b2);
13177         final Matrix ma1 = calibrator.getInitialMa();
13178         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13179         final Matrix ma2 = new Matrix(3, 3);
13180         calibrator.getInitialMa(ma2);
13181         assertEquals(ma1, ma2);
13182         assertSame(calibrator.getEcefPosition(), ecefPosition);
13183         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13184         final NEDPosition nedPosition1 = new NEDPosition();
13185         assertTrue(calibrator.getNedPosition(nedPosition1));
13186         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13187         assertNull(calibrator.getMeasurements());
13188         assertFalse(calibrator.isCommonAxisUsed());
13189         assertNull(calibrator.getListener());
13190         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13191         assertFalse(calibrator.isReady());
13192         assertFalse(calibrator.isRunning());
13193         assertNull(calibrator.getEstimatedBiases());
13194         assertFalse(calibrator.getEstimatedBiases(null));
13195         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13196         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13197         assertNull(calibrator.getEstimatedBiasFx());
13198         assertNull(calibrator.getEstimatedBiasFy());
13199         assertNull(calibrator.getEstimatedBiasFz());
13200         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13201         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13202         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13203         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13204         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13205         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13206         assertNull(calibrator.getEstimatedMa());
13207         assertNull(calibrator.getEstimatedSx());
13208         assertNull(calibrator.getEstimatedSy());
13209         assertNull(calibrator.getEstimatedSz());
13210         assertNull(calibrator.getEstimatedMxy());
13211         assertNull(calibrator.getEstimatedMxz());
13212         assertNull(calibrator.getEstimatedMyx());
13213         assertNull(calibrator.getEstimatedMyz());
13214         assertNull(calibrator.getEstimatedMzx());
13215         assertNull(calibrator.getEstimatedMzy());
13216         assertNull(calibrator.getEstimatedCovariance());
13217         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13218         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13219                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13220         assertNotNull(calibrator.getGroundTruthGravityNorm());
13221         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13222         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13223         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13224                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13225         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13226         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13227         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13228     }
13229 
13230     @Test
13231     public void testConstructor111() throws WrongSizeException {
13232         final Matrix ba = generateBa();
13233         final double biasX = ba.getElementAtIndex(0);
13234         final double biasY = ba.getElementAtIndex(1);
13235         final double biasZ = ba.getElementAtIndex(2);
13236 
13237         final Acceleration bx = new Acceleration(biasX,
13238                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13239         final Acceleration by = new Acceleration(biasY,
13240                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13241         final Acceleration bz = new Acceleration(biasZ,
13242                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13243 
13244         final Matrix ma = generateMaCommonAxis();
13245         final double sx = ma.getElementAt(0, 0);
13246         final double sy = ma.getElementAt(1, 1);
13247         final double sz = ma.getElementAt(2, 2);
13248 
13249         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13250         final double latitude = Math.toRadians(
13251                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13252         final double longitude = Math.toRadians(
13253                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13254         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13255         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13256         final NEDVelocity nedVelocity = new NEDVelocity();
13257         final ECEFPosition ecefPosition = new ECEFPosition();
13258         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13259         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13260                 ecefPosition, ecefVelocity);
13261 
13262         final KnownPositionAccelerometerCalibrator calibrator =
13263                 new KnownPositionAccelerometerCalibrator(ecefPosition,
13264                         bx, by, bz, sx, sy, sz, this);
13265 
13266         // check default values
13267         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13268         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13269         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13270         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13271         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13272         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13273         final Acceleration bx2 = new Acceleration(0.0,
13274                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13275         calibrator.getInitialBiasXAsAcceleration(bx2);
13276         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13277         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13278         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13279         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13280         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13281         final Acceleration by2 = new Acceleration(0.0,
13282                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13283         calibrator.getInitialBiasYAsAcceleration(by2);
13284         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13285         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13286         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13287         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13288         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13289         final Acceleration bz2 = new Acceleration(0.0,
13290                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13291         calibrator.getInitialBiasZAsAcceleration(bz2);
13292         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13293         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13294         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13295         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13296         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13297         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13298         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13299         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13300         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13301         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13302         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13303         final double[] bias1 = calibrator.getInitialBias();
13304         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13305         final double[] bias2 = new double[3];
13306         calibrator.getInitialBias(bias2);
13307         assertArrayEquals(bias1, bias2, 0.0);
13308         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13309         assertEquals(b1, ba);
13310         final Matrix b2 = new Matrix(3, 1);
13311         calibrator.getInitialBiasAsMatrix(b2);
13312         assertEquals(b1, b2);
13313         final Matrix ma1 = calibrator.getInitialMa();
13314         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13315         final Matrix ma2 = new Matrix(3, 3);
13316         calibrator.getInitialMa(ma2);
13317         assertEquals(ma1, ma2);
13318         assertSame(calibrator.getEcefPosition(), ecefPosition);
13319         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13320         final NEDPosition nedPosition1 = new NEDPosition();
13321         assertTrue(calibrator.getNedPosition(nedPosition1));
13322         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13323         assertNull(calibrator.getMeasurements());
13324         assertFalse(calibrator.isCommonAxisUsed());
13325         assertSame(calibrator.getListener(), this);
13326         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13327         assertFalse(calibrator.isReady());
13328         assertFalse(calibrator.isRunning());
13329         assertNull(calibrator.getEstimatedBiases());
13330         assertFalse(calibrator.getEstimatedBiases(null));
13331         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13332         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13333         assertNull(calibrator.getEstimatedBiasFx());
13334         assertNull(calibrator.getEstimatedBiasFy());
13335         assertNull(calibrator.getEstimatedBiasFz());
13336         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13337         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13338         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13339         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13340         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13341         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13342         assertNull(calibrator.getEstimatedMa());
13343         assertNull(calibrator.getEstimatedSx());
13344         assertNull(calibrator.getEstimatedSy());
13345         assertNull(calibrator.getEstimatedSz());
13346         assertNull(calibrator.getEstimatedMxy());
13347         assertNull(calibrator.getEstimatedMxz());
13348         assertNull(calibrator.getEstimatedMyx());
13349         assertNull(calibrator.getEstimatedMyz());
13350         assertNull(calibrator.getEstimatedMzx());
13351         assertNull(calibrator.getEstimatedMzy());
13352         assertNull(calibrator.getEstimatedCovariance());
13353         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13354         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13355                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13356         assertNotNull(calibrator.getGroundTruthGravityNorm());
13357         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13358         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13359         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13360                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13361         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13362         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13363         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13364     }
13365 
13366     @Test
13367     public void testConstructor112() throws WrongSizeException {
13368         final Collection<StandardDeviationBodyKinematics> measurements =
13369                 Collections.emptyList();
13370 
13371         final Matrix ba = generateBa();
13372         final double biasX = ba.getElementAtIndex(0);
13373         final double biasY = ba.getElementAtIndex(1);
13374         final double biasZ = ba.getElementAtIndex(2);
13375 
13376         final Acceleration bx = new Acceleration(biasX,
13377                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13378         final Acceleration by = new Acceleration(biasY,
13379                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13380         final Acceleration bz = new Acceleration(biasZ,
13381                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13382 
13383         final Matrix ma = generateMaCommonAxis();
13384         final double sx = ma.getElementAt(0, 0);
13385         final double sy = ma.getElementAt(1, 1);
13386         final double sz = ma.getElementAt(2, 2);
13387 
13388         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13389         final double latitude = Math.toRadians(
13390                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13391         final double longitude = Math.toRadians(
13392                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13393         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13394         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13395         final NEDVelocity nedVelocity = new NEDVelocity();
13396         final ECEFPosition ecefPosition = new ECEFPosition();
13397         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13398         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13399                 ecefPosition, ecefVelocity);
13400 
13401         final KnownPositionAccelerometerCalibrator calibrator =
13402                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13403                         bx, by, bz, sx, sy, sz);
13404 
13405         // check default values
13406         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13407         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13408         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13409         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13410         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13411         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13412         final Acceleration bx2 = new Acceleration(0.0,
13413                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13414         calibrator.getInitialBiasXAsAcceleration(bx2);
13415         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13416         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13417         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13418         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13419         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13420         final Acceleration by2 = new Acceleration(0.0,
13421                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13422         calibrator.getInitialBiasYAsAcceleration(by2);
13423         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13424         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13425         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13426         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13427         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13428         final Acceleration bz2 = new Acceleration(0.0,
13429                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13430         calibrator.getInitialBiasZAsAcceleration(bz2);
13431         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13432         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13433         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13434         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13435         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13436         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13437         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13438         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13439         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13440         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13441         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13442         final double[] bias1 = calibrator.getInitialBias();
13443         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13444         final double[] bias2 = new double[3];
13445         calibrator.getInitialBias(bias2);
13446         assertArrayEquals(bias1, bias2, 0.0);
13447         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13448         assertEquals(b1, ba);
13449         final Matrix b2 = new Matrix(3, 1);
13450         calibrator.getInitialBiasAsMatrix(b2);
13451         assertEquals(b1, b2);
13452         final Matrix ma1 = calibrator.getInitialMa();
13453         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13454         final Matrix ma2 = new Matrix(3, 3);
13455         calibrator.getInitialMa(ma2);
13456         assertEquals(ma1, ma2);
13457         assertSame(calibrator.getEcefPosition(), ecefPosition);
13458         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13459         final NEDPosition nedPosition1 = new NEDPosition();
13460         assertTrue(calibrator.getNedPosition(nedPosition1));
13461         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13462         assertSame(calibrator.getMeasurements(), measurements);
13463         assertFalse(calibrator.isCommonAxisUsed());
13464         assertNull(calibrator.getListener());
13465         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13466         assertFalse(calibrator.isReady());
13467         assertFalse(calibrator.isRunning());
13468         assertNull(calibrator.getEstimatedBiases());
13469         assertFalse(calibrator.getEstimatedBiases(null));
13470         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13471         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13472         assertNull(calibrator.getEstimatedBiasFx());
13473         assertNull(calibrator.getEstimatedBiasFy());
13474         assertNull(calibrator.getEstimatedBiasFz());
13475         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13476         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13477         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13478         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13479         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13480         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13481         assertNull(calibrator.getEstimatedMa());
13482         assertNull(calibrator.getEstimatedSx());
13483         assertNull(calibrator.getEstimatedSy());
13484         assertNull(calibrator.getEstimatedSz());
13485         assertNull(calibrator.getEstimatedMxy());
13486         assertNull(calibrator.getEstimatedMxz());
13487         assertNull(calibrator.getEstimatedMyx());
13488         assertNull(calibrator.getEstimatedMyz());
13489         assertNull(calibrator.getEstimatedMzx());
13490         assertNull(calibrator.getEstimatedMzy());
13491         assertNull(calibrator.getEstimatedCovariance());
13492         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13493         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13494                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13495         assertNotNull(calibrator.getGroundTruthGravityNorm());
13496         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13497         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13498         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13499                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13500         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13501         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13502         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13503     }
13504 
13505     @Test
13506     public void testConstructor113() throws WrongSizeException {
13507         final Collection<StandardDeviationBodyKinematics> measurements =
13508                 Collections.emptyList();
13509 
13510         final Matrix ba = generateBa();
13511         final double biasX = ba.getElementAtIndex(0);
13512         final double biasY = ba.getElementAtIndex(1);
13513         final double biasZ = ba.getElementAtIndex(2);
13514 
13515         final Acceleration bx = new Acceleration(biasX,
13516                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13517         final Acceleration by = new Acceleration(biasY,
13518                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13519         final Acceleration bz = new Acceleration(biasZ,
13520                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13521 
13522         final Matrix ma = generateMaCommonAxis();
13523         final double sx = ma.getElementAt(0, 0);
13524         final double sy = ma.getElementAt(1, 1);
13525         final double sz = ma.getElementAt(2, 2);
13526 
13527         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13528         final double latitude = Math.toRadians(
13529                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13530         final double longitude = Math.toRadians(
13531                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13532         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13533         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13534         final NEDVelocity nedVelocity = new NEDVelocity();
13535         final ECEFPosition ecefPosition = new ECEFPosition();
13536         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13537         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13538                 ecefPosition, ecefVelocity);
13539 
13540         final KnownPositionAccelerometerCalibrator calibrator =
13541                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13542                         bx, by, bz, sx, sy, sz, this);
13543 
13544         // check default values
13545         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13546         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13547         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13548         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13549         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13550         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13551         final Acceleration bx2 = new Acceleration(0.0,
13552                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13553         calibrator.getInitialBiasXAsAcceleration(bx2);
13554         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13555         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13556         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13557         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13558         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13559         final Acceleration by2 = new Acceleration(0.0,
13560                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13561         calibrator.getInitialBiasYAsAcceleration(by2);
13562         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13563         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13564         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13565         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13566         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13567         final Acceleration bz2 = new Acceleration(0.0,
13568                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13569         calibrator.getInitialBiasZAsAcceleration(bz2);
13570         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13571         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13572         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13573         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13574         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13575         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13576         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13577         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13578         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13579         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13580         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13581         final double[] bias1 = calibrator.getInitialBias();
13582         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13583         final double[] bias2 = new double[3];
13584         calibrator.getInitialBias(bias2);
13585         assertArrayEquals(bias1, bias2, 0.0);
13586         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13587         assertEquals(b1, ba);
13588         final Matrix b2 = new Matrix(3, 1);
13589         calibrator.getInitialBiasAsMatrix(b2);
13590         assertEquals(b1, b2);
13591         final Matrix ma1 = calibrator.getInitialMa();
13592         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13593         final Matrix ma2 = new Matrix(3, 3);
13594         calibrator.getInitialMa(ma2);
13595         assertEquals(ma1, ma2);
13596         assertSame(calibrator.getEcefPosition(), ecefPosition);
13597         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13598         final NEDPosition nedPosition1 = new NEDPosition();
13599         assertTrue(calibrator.getNedPosition(nedPosition1));
13600         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13601         assertSame(calibrator.getMeasurements(), measurements);
13602         assertFalse(calibrator.isCommonAxisUsed());
13603         assertSame(calibrator.getListener(), this);
13604         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13605         assertFalse(calibrator.isReady());
13606         assertFalse(calibrator.isRunning());
13607         assertNull(calibrator.getEstimatedBiases());
13608         assertFalse(calibrator.getEstimatedBiases(null));
13609         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13610         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13611         assertNull(calibrator.getEstimatedBiasFx());
13612         assertNull(calibrator.getEstimatedBiasFy());
13613         assertNull(calibrator.getEstimatedBiasFz());
13614         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13615         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13616         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13617         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13618         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13619         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13620         assertNull(calibrator.getEstimatedMa());
13621         assertNull(calibrator.getEstimatedSx());
13622         assertNull(calibrator.getEstimatedSy());
13623         assertNull(calibrator.getEstimatedSz());
13624         assertNull(calibrator.getEstimatedMxy());
13625         assertNull(calibrator.getEstimatedMxz());
13626         assertNull(calibrator.getEstimatedMyx());
13627         assertNull(calibrator.getEstimatedMyz());
13628         assertNull(calibrator.getEstimatedMzx());
13629         assertNull(calibrator.getEstimatedMzy());
13630         assertNull(calibrator.getEstimatedCovariance());
13631         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13632         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13633                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13634         assertNotNull(calibrator.getGroundTruthGravityNorm());
13635         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13636         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13637         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13638                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13639         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13640         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13641         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13642     }
13643 
13644     @Test
13645     public void testConstructor114() throws WrongSizeException {
13646         final Matrix ba = generateBa();
13647         final double biasX = ba.getElementAtIndex(0);
13648         final double biasY = ba.getElementAtIndex(1);
13649         final double biasZ = ba.getElementAtIndex(2);
13650 
13651         final Acceleration bx = new Acceleration(biasX,
13652                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13653         final Acceleration by = new Acceleration(biasY,
13654                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13655         final Acceleration bz = new Acceleration(biasZ,
13656                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13657 
13658         final Matrix ma = generateMaCommonAxis();
13659         final double sx = ma.getElementAt(0, 0);
13660         final double sy = ma.getElementAt(1, 1);
13661         final double sz = ma.getElementAt(2, 2);
13662 
13663         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13664         final double latitude = Math.toRadians(
13665                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13666         final double longitude = Math.toRadians(
13667                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13668         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13669         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13670         final NEDVelocity nedVelocity = new NEDVelocity();
13671         final ECEFPosition ecefPosition = new ECEFPosition();
13672         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13673         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13674                 ecefPosition, ecefVelocity);
13675 
13676         final KnownPositionAccelerometerCalibrator calibrator =
13677                 new KnownPositionAccelerometerCalibrator(ecefPosition,
13678                         true, bx, by, bz, sx, sy, sz);
13679 
13680         // check default values
13681         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13682         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13683         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13684         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13685         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13686         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13687         final Acceleration bx2 = new Acceleration(0.0,
13688                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13689         calibrator.getInitialBiasXAsAcceleration(bx2);
13690         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13691         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13692         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13693         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13694         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13695         final Acceleration by2 = new Acceleration(0.0,
13696                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13697         calibrator.getInitialBiasYAsAcceleration(by2);
13698         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13699         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13700         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13701         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13702         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13703         final Acceleration bz2 = new Acceleration(0.0,
13704                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13705         calibrator.getInitialBiasZAsAcceleration(bz2);
13706         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13707         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13708         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13709         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13710         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13711         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13712         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13713         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13714         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13715         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13716         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13717         final double[] bias1 = calibrator.getInitialBias();
13718         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13719         final double[] bias2 = new double[3];
13720         calibrator.getInitialBias(bias2);
13721         assertArrayEquals(bias1, bias2, 0.0);
13722         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13723         assertEquals(b1, ba);
13724         final Matrix b2 = new Matrix(3, 1);
13725         calibrator.getInitialBiasAsMatrix(b2);
13726         assertEquals(b1, b2);
13727         final Matrix ma1 = calibrator.getInitialMa();
13728         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13729         final Matrix ma2 = new Matrix(3, 3);
13730         calibrator.getInitialMa(ma2);
13731         assertEquals(ma1, ma2);
13732         assertSame(calibrator.getEcefPosition(), ecefPosition);
13733         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13734         final NEDPosition nedPosition1 = new NEDPosition();
13735         assertTrue(calibrator.getNedPosition(nedPosition1));
13736         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13737         assertNull(calibrator.getMeasurements());
13738         assertTrue(calibrator.isCommonAxisUsed());
13739         assertNull(calibrator.getListener());
13740         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13741         assertFalse(calibrator.isReady());
13742         assertFalse(calibrator.isRunning());
13743         assertNull(calibrator.getEstimatedBiases());
13744         assertFalse(calibrator.getEstimatedBiases(null));
13745         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13746         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13747         assertNull(calibrator.getEstimatedBiasFx());
13748         assertNull(calibrator.getEstimatedBiasFy());
13749         assertNull(calibrator.getEstimatedBiasFz());
13750         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13751         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13752         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13753         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13754         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13755         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13756         assertNull(calibrator.getEstimatedMa());
13757         assertNull(calibrator.getEstimatedSx());
13758         assertNull(calibrator.getEstimatedSy());
13759         assertNull(calibrator.getEstimatedSz());
13760         assertNull(calibrator.getEstimatedMxy());
13761         assertNull(calibrator.getEstimatedMxz());
13762         assertNull(calibrator.getEstimatedMyx());
13763         assertNull(calibrator.getEstimatedMyz());
13764         assertNull(calibrator.getEstimatedMzx());
13765         assertNull(calibrator.getEstimatedMzy());
13766         assertNull(calibrator.getEstimatedCovariance());
13767         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13768         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13769                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13770         assertNotNull(calibrator.getGroundTruthGravityNorm());
13771         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13772         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13773         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13774                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13775         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13776         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13777         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13778     }
13779 
13780     @Test
13781     public void testConstructor115() throws WrongSizeException {
13782         final Matrix ba = generateBa();
13783         final double biasX = ba.getElementAtIndex(0);
13784         final double biasY = ba.getElementAtIndex(1);
13785         final double biasZ = ba.getElementAtIndex(2);
13786 
13787         final Acceleration bx = new Acceleration(biasX,
13788                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13789         final Acceleration by = new Acceleration(biasY,
13790                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13791         final Acceleration bz = new Acceleration(biasZ,
13792                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13793 
13794         final Matrix ma = generateMaCommonAxis();
13795         final double sx = ma.getElementAt(0, 0);
13796         final double sy = ma.getElementAt(1, 1);
13797         final double sz = ma.getElementAt(2, 2);
13798 
13799         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13800         final double latitude = Math.toRadians(
13801                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13802         final double longitude = Math.toRadians(
13803                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13804         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13805         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13806         final NEDVelocity nedVelocity = new NEDVelocity();
13807         final ECEFPosition ecefPosition = new ECEFPosition();
13808         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13809         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13810                 ecefPosition, ecefVelocity);
13811 
13812         final KnownPositionAccelerometerCalibrator calibrator =
13813                 new KnownPositionAccelerometerCalibrator(ecefPosition,
13814                         true, bx, by, bz, sx, sy, sz, this);
13815 
13816         // check default values
13817         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13818         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13819         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13820         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13821         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13822         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13823         final Acceleration bx2 = new Acceleration(0.0,
13824                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13825         calibrator.getInitialBiasXAsAcceleration(bx2);
13826         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13827         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13828         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13829         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13830         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13831         final Acceleration by2 = new Acceleration(0.0,
13832                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13833         calibrator.getInitialBiasYAsAcceleration(by2);
13834         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13835         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13836         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13837         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13838         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13839         final Acceleration bz2 = new Acceleration(0.0,
13840                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13841         calibrator.getInitialBiasZAsAcceleration(bz2);
13842         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13843         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13844         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13845         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13846         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13847         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13848         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13849         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13850         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13851         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13852         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13853         final double[] bias1 = calibrator.getInitialBias();
13854         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13855         final double[] bias2 = new double[3];
13856         calibrator.getInitialBias(bias2);
13857         assertArrayEquals(bias1, bias2, 0.0);
13858         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13859         assertEquals(b1, ba);
13860         final Matrix b2 = new Matrix(3, 1);
13861         calibrator.getInitialBiasAsMatrix(b2);
13862         assertEquals(b1, b2);
13863         final Matrix ma1 = calibrator.getInitialMa();
13864         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13865         final Matrix ma2 = new Matrix(3, 3);
13866         calibrator.getInitialMa(ma2);
13867         assertEquals(ma1, ma2);
13868         assertSame(calibrator.getEcefPosition(), ecefPosition);
13869         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13870         final NEDPosition nedPosition1 = new NEDPosition();
13871         assertTrue(calibrator.getNedPosition(nedPosition1));
13872         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13873         assertNull(calibrator.getMeasurements());
13874         assertTrue(calibrator.isCommonAxisUsed());
13875         assertSame(calibrator.getListener(), this);
13876         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13877         assertFalse(calibrator.isReady());
13878         assertFalse(calibrator.isRunning());
13879         assertNull(calibrator.getEstimatedBiases());
13880         assertFalse(calibrator.getEstimatedBiases(null));
13881         assertNull(calibrator.getEstimatedBiasesAsMatrix());
13882         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13883         assertNull(calibrator.getEstimatedBiasFx());
13884         assertNull(calibrator.getEstimatedBiasFy());
13885         assertNull(calibrator.getEstimatedBiasFz());
13886         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13887         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13888         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13889         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13890         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13891         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13892         assertNull(calibrator.getEstimatedMa());
13893         assertNull(calibrator.getEstimatedSx());
13894         assertNull(calibrator.getEstimatedSy());
13895         assertNull(calibrator.getEstimatedSz());
13896         assertNull(calibrator.getEstimatedMxy());
13897         assertNull(calibrator.getEstimatedMxz());
13898         assertNull(calibrator.getEstimatedMyx());
13899         assertNull(calibrator.getEstimatedMyz());
13900         assertNull(calibrator.getEstimatedMzx());
13901         assertNull(calibrator.getEstimatedMzy());
13902         assertNull(calibrator.getEstimatedCovariance());
13903         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13904         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13905                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13906         assertNotNull(calibrator.getGroundTruthGravityNorm());
13907         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13908         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13909         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13910                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13911         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13912         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13913         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13914     }
13915 
13916     @Test
13917     public void testConstructor116() throws WrongSizeException {
13918         final Collection<StandardDeviationBodyKinematics> measurements =
13919                 Collections.emptyList();
13920 
13921         final Matrix ba = generateBa();
13922         final double biasX = ba.getElementAtIndex(0);
13923         final double biasY = ba.getElementAtIndex(1);
13924         final double biasZ = ba.getElementAtIndex(2);
13925 
13926         final Acceleration bx = new Acceleration(biasX,
13927                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13928         final Acceleration by = new Acceleration(biasY,
13929                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13930         final Acceleration bz = new Acceleration(biasZ,
13931                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13932 
13933         final Matrix ma = generateMaCommonAxis();
13934         final double sx = ma.getElementAt(0, 0);
13935         final double sy = ma.getElementAt(1, 1);
13936         final double sz = ma.getElementAt(2, 2);
13937 
13938         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13939         final double latitude = Math.toRadians(
13940                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13941         final double longitude = Math.toRadians(
13942                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13943         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13944         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13945         final NEDVelocity nedVelocity = new NEDVelocity();
13946         final ECEFPosition ecefPosition = new ECEFPosition();
13947         final ECEFVelocity ecefVelocity = new ECEFVelocity();
13948         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13949                 ecefPosition, ecefVelocity);
13950 
13951         final KnownPositionAccelerometerCalibrator calibrator =
13952                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13953                         true, bx, by, bz, sx, sy, sz);
13954 
13955         // check default values
13956         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13957         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13958         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13959         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13960         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13961         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13962         final Acceleration bx2 = new Acceleration(0.0,
13963                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13964         calibrator.getInitialBiasXAsAcceleration(bx2);
13965         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13966         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13967         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13968         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13969         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13970         final Acceleration by2 = new Acceleration(0.0,
13971                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13972         calibrator.getInitialBiasYAsAcceleration(by2);
13973         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13974         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13975         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13976         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13977         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13978         final Acceleration bz2 = new Acceleration(0.0,
13979                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13980         calibrator.getInitialBiasZAsAcceleration(bz2);
13981         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13982         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13983         assertEquals(calibrator.getInitialSx(), sx, 0.0);
13984         assertEquals(calibrator.getInitialSy(), sy, 0.0);
13985         assertEquals(calibrator.getInitialSz(), sz, 0.0);
13986         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13987         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13988         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13989         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13990         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13991         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13992         final double[] bias1 = calibrator.getInitialBias();
13993         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13994         final double[] bias2 = new double[3];
13995         calibrator.getInitialBias(bias2);
13996         assertArrayEquals(bias1, bias2, 0.0);
13997         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13998         assertEquals(b1, ba);
13999         final Matrix b2 = new Matrix(3, 1);
14000         calibrator.getInitialBiasAsMatrix(b2);
14001         assertEquals(b1, b2);
14002         final Matrix ma1 = calibrator.getInitialMa();
14003         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
14004         final Matrix ma2 = new Matrix(3, 3);
14005         calibrator.getInitialMa(ma2);
14006         assertEquals(ma1, ma2);
14007         assertSame(calibrator.getEcefPosition(), ecefPosition);
14008         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14009         final NEDPosition nedPosition1 = new NEDPosition();
14010         assertTrue(calibrator.getNedPosition(nedPosition1));
14011         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14012         assertSame(calibrator.getMeasurements(), measurements);
14013         assertTrue(calibrator.isCommonAxisUsed());
14014         assertNull(calibrator.getListener());
14015         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14016         assertFalse(calibrator.isReady());
14017         assertFalse(calibrator.isRunning());
14018         assertNull(calibrator.getEstimatedBiases());
14019         assertFalse(calibrator.getEstimatedBiases(null));
14020         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14021         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14022         assertNull(calibrator.getEstimatedBiasFx());
14023         assertNull(calibrator.getEstimatedBiasFy());
14024         assertNull(calibrator.getEstimatedBiasFz());
14025         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14026         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14027         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14028         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14029         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14030         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14031         assertNull(calibrator.getEstimatedMa());
14032         assertNull(calibrator.getEstimatedSx());
14033         assertNull(calibrator.getEstimatedSy());
14034         assertNull(calibrator.getEstimatedSz());
14035         assertNull(calibrator.getEstimatedMxy());
14036         assertNull(calibrator.getEstimatedMxz());
14037         assertNull(calibrator.getEstimatedMyx());
14038         assertNull(calibrator.getEstimatedMyz());
14039         assertNull(calibrator.getEstimatedMzx());
14040         assertNull(calibrator.getEstimatedMzy());
14041         assertNull(calibrator.getEstimatedCovariance());
14042         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14043         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14044                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14045         assertNotNull(calibrator.getGroundTruthGravityNorm());
14046         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14047         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14048         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14049                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14050         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14051         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14052         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14053     }
14054 
14055     @Test
14056     public void testConstructor117() throws WrongSizeException {
14057         final Collection<StandardDeviationBodyKinematics> measurements =
14058                 Collections.emptyList();
14059 
14060         final Matrix ba = generateBa();
14061         final double biasX = ba.getElementAtIndex(0);
14062         final double biasY = ba.getElementAtIndex(1);
14063         final double biasZ = ba.getElementAtIndex(2);
14064 
14065         final Acceleration bx = new Acceleration(biasX,
14066                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14067         final Acceleration by = new Acceleration(biasY,
14068                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14069         final Acceleration bz = new Acceleration(biasZ,
14070                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14071 
14072         final Matrix ma = generateMaCommonAxis();
14073         final double sx = ma.getElementAt(0, 0);
14074         final double sy = ma.getElementAt(1, 1);
14075         final double sz = ma.getElementAt(2, 2);
14076 
14077         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14078         final double latitude = Math.toRadians(
14079                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14080         final double longitude = Math.toRadians(
14081                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14082         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14083         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14084         final NEDVelocity nedVelocity = new NEDVelocity();
14085         final ECEFPosition ecefPosition = new ECEFPosition();
14086         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14087         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14088                 ecefPosition, ecefVelocity);
14089 
14090         final KnownPositionAccelerometerCalibrator calibrator =
14091                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14092                         true, bx, by, bz, sx, sy, sz, this);
14093 
14094         // check default values
14095         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14096         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14097         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14098         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14099         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14100         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14101         final Acceleration bx2 = new Acceleration(0.0,
14102                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14103         calibrator.getInitialBiasXAsAcceleration(bx2);
14104         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14105         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14106         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14107         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14108         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14109         final Acceleration by2 = new Acceleration(0.0,
14110                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14111         calibrator.getInitialBiasYAsAcceleration(by2);
14112         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14113         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14114         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14115         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14116         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14117         final Acceleration bz2 = new Acceleration(0.0,
14118                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14119         calibrator.getInitialBiasZAsAcceleration(bz2);
14120         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14121         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14122         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14123         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14124         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14125         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
14126         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
14127         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
14128         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
14129         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
14130         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
14131         final double[] bias1 = calibrator.getInitialBias();
14132         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14133         final double[] bias2 = new double[3];
14134         calibrator.getInitialBias(bias2);
14135         assertArrayEquals(bias1, bias2, 0.0);
14136         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14137         assertEquals(b1, ba);
14138         final Matrix b2 = new Matrix(3, 1);
14139         calibrator.getInitialBiasAsMatrix(b2);
14140         assertEquals(b1, b2);
14141         final Matrix ma1 = calibrator.getInitialMa();
14142         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
14143         final Matrix ma2 = new Matrix(3, 3);
14144         calibrator.getInitialMa(ma2);
14145         assertEquals(ma1, ma2);
14146         assertSame(calibrator.getEcefPosition(), ecefPosition);
14147         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14148         final NEDPosition nedPosition1 = new NEDPosition();
14149         assertTrue(calibrator.getNedPosition(nedPosition1));
14150         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14151         assertSame(calibrator.getMeasurements(), measurements);
14152         assertTrue(calibrator.isCommonAxisUsed());
14153         assertSame(calibrator.getListener(), this);
14154         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14155         assertFalse(calibrator.isReady());
14156         assertFalse(calibrator.isRunning());
14157         assertNull(calibrator.getEstimatedBiases());
14158         assertFalse(calibrator.getEstimatedBiases(null));
14159         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14160         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14161         assertNull(calibrator.getEstimatedBiasFx());
14162         assertNull(calibrator.getEstimatedBiasFy());
14163         assertNull(calibrator.getEstimatedBiasFz());
14164         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14165         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14166         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14167         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14168         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14169         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14170         assertNull(calibrator.getEstimatedMa());
14171         assertNull(calibrator.getEstimatedSx());
14172         assertNull(calibrator.getEstimatedSy());
14173         assertNull(calibrator.getEstimatedSz());
14174         assertNull(calibrator.getEstimatedMxy());
14175         assertNull(calibrator.getEstimatedMxz());
14176         assertNull(calibrator.getEstimatedMyx());
14177         assertNull(calibrator.getEstimatedMyz());
14178         assertNull(calibrator.getEstimatedMzx());
14179         assertNull(calibrator.getEstimatedMzy());
14180         assertNull(calibrator.getEstimatedCovariance());
14181         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14182         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14183                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14184         assertNotNull(calibrator.getGroundTruthGravityNorm());
14185         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14186         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14187         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14188                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14189         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14190         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14191         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14192     }
14193 
14194     @Test
14195     public void testConstructor118() throws WrongSizeException {
14196         final Matrix ba = generateBa();
14197         final double biasX = ba.getElementAtIndex(0);
14198         final double biasY = ba.getElementAtIndex(1);
14199         final double biasZ = ba.getElementAtIndex(2);
14200 
14201         final Matrix ma = generateMaCommonAxis();
14202         final double sx = ma.getElementAt(0, 0);
14203         final double sy = ma.getElementAt(1, 1);
14204         final double sz = ma.getElementAt(2, 2);
14205         final double mxy = ma.getElementAt(0, 1);
14206         final double mxz = ma.getElementAt(0, 2);
14207         final double myx = ma.getElementAt(1, 0);
14208         final double myz = ma.getElementAt(1, 2);
14209         final double mzx = ma.getElementAt(2, 0);
14210         final double mzy = ma.getElementAt(2, 1);
14211 
14212         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14213         final double latitude = Math.toRadians(
14214                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14215         final double longitude = Math.toRadians(
14216                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14217         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14218         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14219         final NEDVelocity nedVelocity = new NEDVelocity();
14220         final ECEFPosition ecefPosition = new ECEFPosition();
14221         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14222         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14223                 ecefPosition, ecefVelocity);
14224 
14225         final KnownPositionAccelerometerCalibrator calibrator =
14226                 new KnownPositionAccelerometerCalibrator(ecefPosition,
14227                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14228                         myx, myz, mzx, mzy);
14229 
14230         // check default values
14231         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14232         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14233         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14234         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14235         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14236         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14237         final Acceleration bx2 = new Acceleration(0.0,
14238                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14239         calibrator.getInitialBiasXAsAcceleration(bx2);
14240         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14241         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14242         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14243         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14244         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14245         final Acceleration by2 = new Acceleration(0.0,
14246                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14247         calibrator.getInitialBiasYAsAcceleration(by2);
14248         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14249         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14250         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14251         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14252         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14253         final Acceleration bz2 = new Acceleration(0.0,
14254                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14255         calibrator.getInitialBiasZAsAcceleration(bz2);
14256         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14257         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14258         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14259         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14260         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14261         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14262         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14263         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14264         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14265         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14266         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14267         final double[] bias1 = calibrator.getInitialBias();
14268         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14269         final double[] bias2 = new double[3];
14270         calibrator.getInitialBias(bias2);
14271         assertArrayEquals(bias1, bias2, 0.0);
14272         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14273         assertEquals(b1, ba);
14274         final Matrix b2 = new Matrix(3, 1);
14275         calibrator.getInitialBiasAsMatrix(b2);
14276         assertEquals(b1, b2);
14277         final Matrix ma1 = new Matrix(3, 3);
14278         ma1.setSubmatrix(0, 0,
14279                 2, 2,
14280                 new double[]{sx, myx, mzx,
14281                         mxy, sy, mzy,
14282                         mxz, myz, sz});
14283         assertEquals(calibrator.getInitialMa(), ma1);
14284         final Matrix ma2 = new Matrix(3, 3);
14285         calibrator.getInitialMa(ma2);
14286         assertEquals(ma1, ma2);
14287         assertSame(calibrator.getEcefPosition(), ecefPosition);
14288         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14289         final NEDPosition nedPosition1 = new NEDPosition();
14290         assertTrue(calibrator.getNedPosition(nedPosition1));
14291         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14292         assertNull(calibrator.getMeasurements());
14293         assertFalse(calibrator.isCommonAxisUsed());
14294         assertNull(calibrator.getListener());
14295         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14296         assertFalse(calibrator.isReady());
14297         assertFalse(calibrator.isRunning());
14298         assertNull(calibrator.getEstimatedBiases());
14299         assertFalse(calibrator.getEstimatedBiases(null));
14300         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14301         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14302         assertNull(calibrator.getEstimatedBiasFx());
14303         assertNull(calibrator.getEstimatedBiasFy());
14304         assertNull(calibrator.getEstimatedBiasFz());
14305         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14306         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14307         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14308         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14309         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14310         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14311         assertNull(calibrator.getEstimatedMa());
14312         assertNull(calibrator.getEstimatedSx());
14313         assertNull(calibrator.getEstimatedSy());
14314         assertNull(calibrator.getEstimatedSz());
14315         assertNull(calibrator.getEstimatedMxy());
14316         assertNull(calibrator.getEstimatedMxz());
14317         assertNull(calibrator.getEstimatedMyx());
14318         assertNull(calibrator.getEstimatedMyz());
14319         assertNull(calibrator.getEstimatedMzx());
14320         assertNull(calibrator.getEstimatedMzy());
14321         assertNull(calibrator.getEstimatedCovariance());
14322         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14323         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14324                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14325         assertNotNull(calibrator.getGroundTruthGravityNorm());
14326         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14327         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14328         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14329                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14330         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14331         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14332         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14333     }
14334 
14335     @Test
14336     public void testConstructor119() throws WrongSizeException {
14337         final Collection<StandardDeviationBodyKinematics> measurements =
14338                 Collections.emptyList();
14339 
14340         final Matrix ba = generateBa();
14341         final double biasX = ba.getElementAtIndex(0);
14342         final double biasY = ba.getElementAtIndex(1);
14343         final double biasZ = ba.getElementAtIndex(2);
14344 
14345         final Matrix ma = generateMaCommonAxis();
14346         final double sx = ma.getElementAt(0, 0);
14347         final double sy = ma.getElementAt(1, 1);
14348         final double sz = ma.getElementAt(2, 2);
14349         final double mxy = ma.getElementAt(0, 1);
14350         final double mxz = ma.getElementAt(0, 2);
14351         final double myx = ma.getElementAt(1, 0);
14352         final double myz = ma.getElementAt(1, 2);
14353         final double mzx = ma.getElementAt(2, 0);
14354         final double mzy = ma.getElementAt(2, 1);
14355 
14356         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14357         final double latitude = Math.toRadians(
14358                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14359         final double longitude = Math.toRadians(
14360                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14361         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14362         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14363         final NEDVelocity nedVelocity = new NEDVelocity();
14364         final ECEFPosition ecefPosition = new ECEFPosition();
14365         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14366         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14367                 ecefPosition, ecefVelocity);
14368 
14369         final KnownPositionAccelerometerCalibrator calibrator =
14370                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14371                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14372                         myx, myz, mzx, mzy);
14373 
14374         // check default values
14375         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14376         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14377         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14378         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14379         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14380         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14381         final Acceleration bx2 = new Acceleration(0.0,
14382                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14383         calibrator.getInitialBiasXAsAcceleration(bx2);
14384         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14385         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14386         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14387         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14388         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14389         final Acceleration by2 = new Acceleration(0.0,
14390                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14391         calibrator.getInitialBiasYAsAcceleration(by2);
14392         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14393         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14394         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14395         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14396         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14397         final Acceleration bz2 = new Acceleration(0.0,
14398                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14399         calibrator.getInitialBiasZAsAcceleration(bz2);
14400         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14401         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14402         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14403         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14404         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14405         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14406         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14407         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14408         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14409         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14410         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14411         final double[] bias1 = calibrator.getInitialBias();
14412         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14413         final double[] bias2 = new double[3];
14414         calibrator.getInitialBias(bias2);
14415         assertArrayEquals(bias1, bias2, 0.0);
14416         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14417         assertEquals(b1, ba);
14418         final Matrix b2 = new Matrix(3, 1);
14419         calibrator.getInitialBiasAsMatrix(b2);
14420         assertEquals(b1, b2);
14421         final Matrix ma1 = new Matrix(3, 3);
14422         ma1.setSubmatrix(0, 0,
14423                 2, 2,
14424                 new double[]{sx, myx, mzx,
14425                         mxy, sy, mzy,
14426                         mxz, myz, sz});
14427         assertEquals(calibrator.getInitialMa(), ma1);
14428         final Matrix ma2 = new Matrix(3, 3);
14429         calibrator.getInitialMa(ma2);
14430         assertEquals(ma1, ma2);
14431         assertSame(calibrator.getEcefPosition(), ecefPosition);
14432         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14433         final NEDPosition nedPosition1 = new NEDPosition();
14434         assertTrue(calibrator.getNedPosition(nedPosition1));
14435         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14436         assertSame(calibrator.getMeasurements(), measurements);
14437         assertFalse(calibrator.isCommonAxisUsed());
14438         assertNull(calibrator.getListener());
14439         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14440         assertFalse(calibrator.isReady());
14441         assertFalse(calibrator.isRunning());
14442         assertNull(calibrator.getEstimatedBiases());
14443         assertFalse(calibrator.getEstimatedBiases(null));
14444         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14445         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14446         assertNull(calibrator.getEstimatedBiasFx());
14447         assertNull(calibrator.getEstimatedBiasFy());
14448         assertNull(calibrator.getEstimatedBiasFz());
14449         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14450         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14451         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14452         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14453         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14454         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14455         assertNull(calibrator.getEstimatedMa());
14456         assertNull(calibrator.getEstimatedSx());
14457         assertNull(calibrator.getEstimatedSy());
14458         assertNull(calibrator.getEstimatedSz());
14459         assertNull(calibrator.getEstimatedMxy());
14460         assertNull(calibrator.getEstimatedMxz());
14461         assertNull(calibrator.getEstimatedMyx());
14462         assertNull(calibrator.getEstimatedMyz());
14463         assertNull(calibrator.getEstimatedMzx());
14464         assertNull(calibrator.getEstimatedMzy());
14465         assertNull(calibrator.getEstimatedCovariance());
14466         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14467         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14468                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14469         assertNotNull(calibrator.getGroundTruthGravityNorm());
14470         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14471         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14472         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14473                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14474         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14475         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14476         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14477     }
14478 
14479     @Test
14480     public void testConstructor120() throws WrongSizeException {
14481         final Collection<StandardDeviationBodyKinematics> measurements =
14482                 Collections.emptyList();
14483 
14484         final Matrix ba = generateBa();
14485         final double biasX = ba.getElementAtIndex(0);
14486         final double biasY = ba.getElementAtIndex(1);
14487         final double biasZ = ba.getElementAtIndex(2);
14488 
14489         final Matrix ma = generateMaCommonAxis();
14490         final double sx = ma.getElementAt(0, 0);
14491         final double sy = ma.getElementAt(1, 1);
14492         final double sz = ma.getElementAt(2, 2);
14493         final double mxy = ma.getElementAt(0, 1);
14494         final double mxz = ma.getElementAt(0, 2);
14495         final double myx = ma.getElementAt(1, 0);
14496         final double myz = ma.getElementAt(1, 2);
14497         final double mzx = ma.getElementAt(2, 0);
14498         final double mzy = ma.getElementAt(2, 1);
14499 
14500         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14501         final double latitude = Math.toRadians(
14502                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14503         final double longitude = Math.toRadians(
14504                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14505         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14506         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14507         final NEDVelocity nedVelocity = new NEDVelocity();
14508         final ECEFPosition ecefPosition = new ECEFPosition();
14509         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14510         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14511                 ecefPosition, ecefVelocity);
14512 
14513         final KnownPositionAccelerometerCalibrator calibrator =
14514                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14515                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14516                         myx, myz, mzx, mzy, this);
14517 
14518         // check default values
14519         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14520         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14521         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14522         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14523         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14524         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14525         final Acceleration bx2 = new Acceleration(0.0,
14526                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14527         calibrator.getInitialBiasXAsAcceleration(bx2);
14528         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14529         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14530         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14531         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14532         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14533         final Acceleration by2 = new Acceleration(0.0,
14534                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14535         calibrator.getInitialBiasYAsAcceleration(by2);
14536         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14537         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14538         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14539         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14540         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14541         final Acceleration bz2 = new Acceleration(0.0,
14542                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14543         calibrator.getInitialBiasZAsAcceleration(bz2);
14544         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14545         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14546         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14547         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14548         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14549         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14550         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14551         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14552         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14553         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14554         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14555         final double[] bias1 = calibrator.getInitialBias();
14556         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14557         final double[] bias2 = new double[3];
14558         calibrator.getInitialBias(bias2);
14559         assertArrayEquals(bias1, bias2, 0.0);
14560         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14561         assertEquals(b1, ba);
14562         final Matrix b2 = new Matrix(3, 1);
14563         calibrator.getInitialBiasAsMatrix(b2);
14564         assertEquals(b1, b2);
14565         final Matrix ma1 = new Matrix(3, 3);
14566         ma1.setSubmatrix(0, 0,
14567                 2, 2,
14568                 new double[]{sx, myx, mzx,
14569                         mxy, sy, mzy,
14570                         mxz, myz, sz});
14571         assertEquals(calibrator.getInitialMa(), ma1);
14572         final Matrix ma2 = new Matrix(3, 3);
14573         calibrator.getInitialMa(ma2);
14574         assertEquals(ma1, ma2);
14575         assertSame(calibrator.getEcefPosition(), ecefPosition);
14576         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14577         final NEDPosition nedPosition1 = new NEDPosition();
14578         assertTrue(calibrator.getNedPosition(nedPosition1));
14579         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14580         assertSame(calibrator.getMeasurements(), measurements);
14581         assertFalse(calibrator.isCommonAxisUsed());
14582         assertSame(calibrator.getListener(), this);
14583         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14584         assertFalse(calibrator.isReady());
14585         assertFalse(calibrator.isRunning());
14586         assertNull(calibrator.getEstimatedBiases());
14587         assertFalse(calibrator.getEstimatedBiases(null));
14588         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14589         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14590         assertNull(calibrator.getEstimatedBiasFx());
14591         assertNull(calibrator.getEstimatedBiasFy());
14592         assertNull(calibrator.getEstimatedBiasFz());
14593         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14594         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14595         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14596         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14597         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14598         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14599         assertNull(calibrator.getEstimatedMa());
14600         assertNull(calibrator.getEstimatedSx());
14601         assertNull(calibrator.getEstimatedSy());
14602         assertNull(calibrator.getEstimatedSz());
14603         assertNull(calibrator.getEstimatedMxy());
14604         assertNull(calibrator.getEstimatedMxz());
14605         assertNull(calibrator.getEstimatedMyx());
14606         assertNull(calibrator.getEstimatedMyz());
14607         assertNull(calibrator.getEstimatedMzx());
14608         assertNull(calibrator.getEstimatedMzy());
14609         assertNull(calibrator.getEstimatedCovariance());
14610         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14611         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14612                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14613         assertNotNull(calibrator.getGroundTruthGravityNorm());
14614         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14615         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14616         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14617                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14618         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14619         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14620         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14621     }
14622 
14623     @Test
14624     public void testConstructor121() throws WrongSizeException {
14625         final Matrix ba = generateBa();
14626         final double biasX = ba.getElementAtIndex(0);
14627         final double biasY = ba.getElementAtIndex(1);
14628         final double biasZ = ba.getElementAtIndex(2);
14629 
14630         final Matrix ma = generateMaCommonAxis();
14631         final double sx = ma.getElementAt(0, 0);
14632         final double sy = ma.getElementAt(1, 1);
14633         final double sz = ma.getElementAt(2, 2);
14634         final double mxy = ma.getElementAt(0, 1);
14635         final double mxz = ma.getElementAt(0, 2);
14636         final double myx = ma.getElementAt(1, 0);
14637         final double myz = ma.getElementAt(1, 2);
14638         final double mzx = ma.getElementAt(2, 0);
14639         final double mzy = ma.getElementAt(2, 1);
14640 
14641         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14642         final double latitude = Math.toRadians(
14643                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14644         final double longitude = Math.toRadians(
14645                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14646         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14647         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14648         final NEDVelocity nedVelocity = new NEDVelocity();
14649         final ECEFPosition ecefPosition = new ECEFPosition();
14650         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14651         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14652                 ecefPosition, ecefVelocity);
14653 
14654         final KnownPositionAccelerometerCalibrator calibrator =
14655                 new KnownPositionAccelerometerCalibrator(ecefPosition,
14656                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14657                         myx, myz, mzx, mzy);
14658 
14659         // check default values
14660         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14661         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14662         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14663         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14664         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14665         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14666         final Acceleration bx2 = new Acceleration(0.0,
14667                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14668         calibrator.getInitialBiasXAsAcceleration(bx2);
14669         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14670         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14671         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14672         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14673         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14674         final Acceleration by2 = new Acceleration(0.0,
14675                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14676         calibrator.getInitialBiasYAsAcceleration(by2);
14677         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14678         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14679         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14680         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14681         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14682         final Acceleration bz2 = new Acceleration(0.0,
14683                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14684         calibrator.getInitialBiasZAsAcceleration(bz2);
14685         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14686         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14687         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14688         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14689         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14690         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14691         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14692         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14693         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14694         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14695         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14696         final double[] bias1 = calibrator.getInitialBias();
14697         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14698         final double[] bias2 = new double[3];
14699         calibrator.getInitialBias(bias2);
14700         assertArrayEquals(bias1, bias2, 0.0);
14701         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14702         assertEquals(b1, ba);
14703         final Matrix b2 = new Matrix(3, 1);
14704         calibrator.getInitialBiasAsMatrix(b2);
14705         assertEquals(b1, b2);
14706         final Matrix ma1 = new Matrix(3, 3);
14707         ma1.setSubmatrix(0, 0,
14708                 2, 2,
14709                 new double[]{sx, myx, mzx,
14710                         mxy, sy, mzy,
14711                         mxz, myz, sz});
14712         assertEquals(calibrator.getInitialMa(), ma1);
14713         final Matrix ma2 = new Matrix(3, 3);
14714         calibrator.getInitialMa(ma2);
14715         assertEquals(ma1, ma2);
14716         assertSame(calibrator.getEcefPosition(), ecefPosition);
14717         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14718         final NEDPosition nedPosition1 = new NEDPosition();
14719         assertTrue(calibrator.getNedPosition(nedPosition1));
14720         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14721         assertNull(calibrator.getMeasurements());
14722         assertTrue(calibrator.isCommonAxisUsed());
14723         assertNull(calibrator.getListener());
14724         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14725         assertFalse(calibrator.isReady());
14726         assertFalse(calibrator.isRunning());
14727         assertNull(calibrator.getEstimatedBiases());
14728         assertFalse(calibrator.getEstimatedBiases(null));
14729         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14730         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14731         assertNull(calibrator.getEstimatedBiasFx());
14732         assertNull(calibrator.getEstimatedBiasFy());
14733         assertNull(calibrator.getEstimatedBiasFz());
14734         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14735         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14736         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14737         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14738         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14739         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14740         assertNull(calibrator.getEstimatedMa());
14741         assertNull(calibrator.getEstimatedSx());
14742         assertNull(calibrator.getEstimatedSy());
14743         assertNull(calibrator.getEstimatedSz());
14744         assertNull(calibrator.getEstimatedMxy());
14745         assertNull(calibrator.getEstimatedMxz());
14746         assertNull(calibrator.getEstimatedMyx());
14747         assertNull(calibrator.getEstimatedMyz());
14748         assertNull(calibrator.getEstimatedMzx());
14749         assertNull(calibrator.getEstimatedMzy());
14750         assertNull(calibrator.getEstimatedCovariance());
14751         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14752         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14753                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14754         assertNotNull(calibrator.getGroundTruthGravityNorm());
14755         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14756         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14757         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14758                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14759         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14760         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14761         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14762     }
14763 
14764     @Test
14765     public void testConstructor122() throws WrongSizeException {
14766         final Matrix ba = generateBa();
14767         final double biasX = ba.getElementAtIndex(0);
14768         final double biasY = ba.getElementAtIndex(1);
14769         final double biasZ = ba.getElementAtIndex(2);
14770 
14771         final Matrix ma = generateMaCommonAxis();
14772         final double sx = ma.getElementAt(0, 0);
14773         final double sy = ma.getElementAt(1, 1);
14774         final double sz = ma.getElementAt(2, 2);
14775         final double mxy = ma.getElementAt(0, 1);
14776         final double mxz = ma.getElementAt(0, 2);
14777         final double myx = ma.getElementAt(1, 0);
14778         final double myz = ma.getElementAt(1, 2);
14779         final double mzx = ma.getElementAt(2, 0);
14780         final double mzy = ma.getElementAt(2, 1);
14781 
14782         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14783         final double latitude = Math.toRadians(
14784                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14785         final double longitude = Math.toRadians(
14786                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14787         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14788         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14789         final NEDVelocity nedVelocity = new NEDVelocity();
14790         final ECEFPosition ecefPosition = new ECEFPosition();
14791         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14792         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14793                 ecefPosition, ecefVelocity);
14794 
14795         final KnownPositionAccelerometerCalibrator calibrator =
14796                 new KnownPositionAccelerometerCalibrator(ecefPosition,
14797                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14798                         myx, myz, mzx, mzy, this);
14799 
14800         // check default values
14801         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14802         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14803         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14804         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14805         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14806         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14807         final Acceleration bx2 = new Acceleration(0.0,
14808                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14809         calibrator.getInitialBiasXAsAcceleration(bx2);
14810         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14811         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14812         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14813         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14814         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14815         final Acceleration by2 = new Acceleration(0.0,
14816                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14817         calibrator.getInitialBiasYAsAcceleration(by2);
14818         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14819         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14820         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14821         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14822         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14823         final Acceleration bz2 = new Acceleration(0.0,
14824                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14825         calibrator.getInitialBiasZAsAcceleration(bz2);
14826         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14827         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14828         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14829         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14830         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14831         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14832         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14833         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14834         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14835         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14836         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14837         final double[] bias1 = calibrator.getInitialBias();
14838         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14839         final double[] bias2 = new double[3];
14840         calibrator.getInitialBias(bias2);
14841         assertArrayEquals(bias1, bias2, 0.0);
14842         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14843         assertEquals(b1, ba);
14844         final Matrix b2 = new Matrix(3, 1);
14845         calibrator.getInitialBiasAsMatrix(b2);
14846         assertEquals(b1, b2);
14847         final Matrix ma1 = new Matrix(3, 3);
14848         ma1.setSubmatrix(0, 0,
14849                 2, 2,
14850                 new double[]{sx, myx, mzx,
14851                         mxy, sy, mzy,
14852                         mxz, myz, sz});
14853         assertEquals(calibrator.getInitialMa(), ma1);
14854         final Matrix ma2 = new Matrix(3, 3);
14855         calibrator.getInitialMa(ma2);
14856         assertEquals(ma1, ma2);
14857         assertSame(calibrator.getEcefPosition(), ecefPosition);
14858         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14859         final NEDPosition nedPosition1 = new NEDPosition();
14860         assertTrue(calibrator.getNedPosition(nedPosition1));
14861         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14862         assertNull(calibrator.getMeasurements());
14863         assertTrue(calibrator.isCommonAxisUsed());
14864         assertSame(calibrator.getListener(), this);
14865         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14866         assertFalse(calibrator.isReady());
14867         assertFalse(calibrator.isRunning());
14868         assertNull(calibrator.getEstimatedBiases());
14869         assertFalse(calibrator.getEstimatedBiases(null));
14870         assertNull(calibrator.getEstimatedBiasesAsMatrix());
14871         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14872         assertNull(calibrator.getEstimatedBiasFx());
14873         assertNull(calibrator.getEstimatedBiasFy());
14874         assertNull(calibrator.getEstimatedBiasFz());
14875         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14876         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14877         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14878         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14879         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14880         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14881         assertNull(calibrator.getEstimatedMa());
14882         assertNull(calibrator.getEstimatedSx());
14883         assertNull(calibrator.getEstimatedSy());
14884         assertNull(calibrator.getEstimatedSz());
14885         assertNull(calibrator.getEstimatedMxy());
14886         assertNull(calibrator.getEstimatedMxz());
14887         assertNull(calibrator.getEstimatedMyx());
14888         assertNull(calibrator.getEstimatedMyz());
14889         assertNull(calibrator.getEstimatedMzx());
14890         assertNull(calibrator.getEstimatedMzy());
14891         assertNull(calibrator.getEstimatedCovariance());
14892         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14893         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14894                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14895         assertNotNull(calibrator.getGroundTruthGravityNorm());
14896         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14897         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14898         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14899                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14900         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14901         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14902         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14903     }
14904 
14905     @Test
14906     public void testConstructor123() throws WrongSizeException {
14907         final Collection<StandardDeviationBodyKinematics> measurements =
14908                 Collections.emptyList();
14909 
14910         final Matrix ba = generateBa();
14911         final double biasX = ba.getElementAtIndex(0);
14912         final double biasY = ba.getElementAtIndex(1);
14913         final double biasZ = ba.getElementAtIndex(2);
14914 
14915         final Matrix ma = generateMaCommonAxis();
14916         final double sx = ma.getElementAt(0, 0);
14917         final double sy = ma.getElementAt(1, 1);
14918         final double sz = ma.getElementAt(2, 2);
14919         final double mxy = ma.getElementAt(0, 1);
14920         final double mxz = ma.getElementAt(0, 2);
14921         final double myx = ma.getElementAt(1, 0);
14922         final double myz = ma.getElementAt(1, 2);
14923         final double mzx = ma.getElementAt(2, 0);
14924         final double mzy = ma.getElementAt(2, 1);
14925 
14926         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14927         final double latitude = Math.toRadians(
14928                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14929         final double longitude = Math.toRadians(
14930                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14931         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14932         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14933         final NEDVelocity nedVelocity = new NEDVelocity();
14934         final ECEFPosition ecefPosition = new ECEFPosition();
14935         final ECEFVelocity ecefVelocity = new ECEFVelocity();
14936         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14937                 ecefPosition, ecefVelocity);
14938 
14939         final KnownPositionAccelerometerCalibrator calibrator =
14940                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14941                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14942                         myx, myz, mzx, mzy);
14943 
14944         // check default values
14945         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14946         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14947         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14948         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14949         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14950         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14951         final Acceleration bx2 = new Acceleration(0.0,
14952                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14953         calibrator.getInitialBiasXAsAcceleration(bx2);
14954         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14955         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14956         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14957         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14958         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14959         final Acceleration by2 = new Acceleration(0.0,
14960                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14961         calibrator.getInitialBiasYAsAcceleration(by2);
14962         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14963         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14964         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14965         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14966         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14967         final Acceleration bz2 = new Acceleration(0.0,
14968                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14969         calibrator.getInitialBiasZAsAcceleration(bz2);
14970         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14971         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14972         assertEquals(calibrator.getInitialSx(), sx, 0.0);
14973         assertEquals(calibrator.getInitialSy(), sy, 0.0);
14974         assertEquals(calibrator.getInitialSz(), sz, 0.0);
14975         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14976         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14977         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14978         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14979         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14980         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14981         final double[] bias1 = calibrator.getInitialBias();
14982         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14983         final double[] bias2 = new double[3];
14984         calibrator.getInitialBias(bias2);
14985         assertArrayEquals(bias1, bias2, 0.0);
14986         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14987         assertEquals(b1, ba);
14988         final Matrix b2 = new Matrix(3, 1);
14989         calibrator.getInitialBiasAsMatrix(b2);
14990         assertEquals(b1, b2);
14991         final Matrix ma1 = new Matrix(3, 3);
14992         ma1.setSubmatrix(0, 0,
14993                 2, 2,
14994                 new double[]{sx, myx, mzx,
14995                         mxy, sy, mzy,
14996                         mxz, myz, sz});
14997         assertEquals(calibrator.getInitialMa(), ma1);
14998         final Matrix ma2 = new Matrix(3, 3);
14999         calibrator.getInitialMa(ma2);
15000         assertEquals(ma1, ma2);
15001         assertSame(calibrator.getEcefPosition(), ecefPosition);
15002         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15003         final NEDPosition nedPosition1 = new NEDPosition();
15004         assertTrue(calibrator.getNedPosition(nedPosition1));
15005         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15006         assertSame(calibrator.getMeasurements(), measurements);
15007         assertTrue(calibrator.isCommonAxisUsed());
15008         assertNull(calibrator.getListener());
15009         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15010         assertFalse(calibrator.isReady());
15011         assertFalse(calibrator.isRunning());
15012         assertNull(calibrator.getEstimatedBiases());
15013         assertFalse(calibrator.getEstimatedBiases(null));
15014         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15015         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15016         assertNull(calibrator.getEstimatedBiasFx());
15017         assertNull(calibrator.getEstimatedBiasFy());
15018         assertNull(calibrator.getEstimatedBiasFz());
15019         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15020         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15021         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15022         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15023         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15024         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15025         assertNull(calibrator.getEstimatedMa());
15026         assertNull(calibrator.getEstimatedSx());
15027         assertNull(calibrator.getEstimatedSy());
15028         assertNull(calibrator.getEstimatedSz());
15029         assertNull(calibrator.getEstimatedMxy());
15030         assertNull(calibrator.getEstimatedMxz());
15031         assertNull(calibrator.getEstimatedMyx());
15032         assertNull(calibrator.getEstimatedMyz());
15033         assertNull(calibrator.getEstimatedMzx());
15034         assertNull(calibrator.getEstimatedMzy());
15035         assertNull(calibrator.getEstimatedCovariance());
15036         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15037         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15038                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15039         assertNotNull(calibrator.getGroundTruthGravityNorm());
15040         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15041         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15042         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15043                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15044         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15045         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15046         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15047     }
15048 
15049     @Test
15050     public void testConstructor124() throws WrongSizeException {
15051         final Collection<StandardDeviationBodyKinematics> measurements =
15052                 Collections.emptyList();
15053 
15054         final Matrix ba = generateBa();
15055         final double biasX = ba.getElementAtIndex(0);
15056         final double biasY = ba.getElementAtIndex(1);
15057         final double biasZ = ba.getElementAtIndex(2);
15058 
15059         final Matrix ma = generateMaCommonAxis();
15060         final double sx = ma.getElementAt(0, 0);
15061         final double sy = ma.getElementAt(1, 1);
15062         final double sz = ma.getElementAt(2, 2);
15063         final double mxy = ma.getElementAt(0, 1);
15064         final double mxz = ma.getElementAt(0, 2);
15065         final double myx = ma.getElementAt(1, 0);
15066         final double myz = ma.getElementAt(1, 2);
15067         final double mzx = ma.getElementAt(2, 0);
15068         final double mzy = ma.getElementAt(2, 1);
15069 
15070         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15071         final double latitude = Math.toRadians(
15072                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15073         final double longitude = Math.toRadians(
15074                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15075         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15076         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15077         final NEDVelocity nedVelocity = new NEDVelocity();
15078         final ECEFPosition ecefPosition = new ECEFPosition();
15079         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15080         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15081                 ecefPosition, ecefVelocity);
15082 
15083         final KnownPositionAccelerometerCalibrator calibrator =
15084                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15085                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
15086                         myx, myz, mzx, mzy, this);
15087 
15088         // check default values
15089         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15090         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15091         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15092         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15093         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15094         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15095         final Acceleration bx2 = new Acceleration(0.0,
15096                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15097         calibrator.getInitialBiasXAsAcceleration(bx2);
15098         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15099         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15100         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15101         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15102         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15103         final Acceleration by2 = new Acceleration(0.0,
15104                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15105         calibrator.getInitialBiasYAsAcceleration(by2);
15106         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15107         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15108         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15109         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15110         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15111         final Acceleration bz2 = new Acceleration(0.0,
15112                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15113         calibrator.getInitialBiasZAsAcceleration(bz2);
15114         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15115         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15116         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15117         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15118         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15119         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15120         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15121         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15122         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15123         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15124         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15125         final double[] bias1 = calibrator.getInitialBias();
15126         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15127         final double[] bias2 = new double[3];
15128         calibrator.getInitialBias(bias2);
15129         assertArrayEquals(bias1, bias2, 0.0);
15130         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15131         assertEquals(b1, ba);
15132         final Matrix b2 = new Matrix(3, 1);
15133         calibrator.getInitialBiasAsMatrix(b2);
15134         assertEquals(b1, b2);
15135         final Matrix ma1 = new Matrix(3, 3);
15136         ma1.setSubmatrix(0, 0,
15137                 2, 2,
15138                 new double[]{sx, myx, mzx,
15139                         mxy, sy, mzy,
15140                         mxz, myz, sz});
15141         assertEquals(calibrator.getInitialMa(), ma1);
15142         final Matrix ma2 = new Matrix(3, 3);
15143         calibrator.getInitialMa(ma2);
15144         assertEquals(ma1, ma2);
15145         assertSame(calibrator.getEcefPosition(), ecefPosition);
15146         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15147         final NEDPosition nedPosition1 = new NEDPosition();
15148         assertTrue(calibrator.getNedPosition(nedPosition1));
15149         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15150         assertSame(calibrator.getMeasurements(), measurements);
15151         assertTrue(calibrator.isCommonAxisUsed());
15152         assertSame(calibrator.getListener(), this);
15153         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15154         assertFalse(calibrator.isReady());
15155         assertFalse(calibrator.isRunning());
15156         assertNull(calibrator.getEstimatedBiases());
15157         assertFalse(calibrator.getEstimatedBiases(null));
15158         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15159         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15160         assertNull(calibrator.getEstimatedBiasFx());
15161         assertNull(calibrator.getEstimatedBiasFy());
15162         assertNull(calibrator.getEstimatedBiasFz());
15163         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15164         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15165         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15166         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15167         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15168         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15169         assertNull(calibrator.getEstimatedMa());
15170         assertNull(calibrator.getEstimatedSx());
15171         assertNull(calibrator.getEstimatedSy());
15172         assertNull(calibrator.getEstimatedSz());
15173         assertNull(calibrator.getEstimatedMxy());
15174         assertNull(calibrator.getEstimatedMxz());
15175         assertNull(calibrator.getEstimatedMyx());
15176         assertNull(calibrator.getEstimatedMyz());
15177         assertNull(calibrator.getEstimatedMzx());
15178         assertNull(calibrator.getEstimatedMzy());
15179         assertNull(calibrator.getEstimatedCovariance());
15180         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15181         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15182                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15183         assertNotNull(calibrator.getGroundTruthGravityNorm());
15184         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15185         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15186         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15187                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15188         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15189         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15190         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15191     }
15192 
15193     @Test
15194     public void testConstructor125() throws WrongSizeException {
15195         final Matrix ba = generateBa();
15196         final double biasX = ba.getElementAtIndex(0);
15197         final double biasY = ba.getElementAtIndex(1);
15198         final double biasZ = ba.getElementAtIndex(2);
15199 
15200         final Acceleration bx = new Acceleration(biasX,
15201                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15202         final Acceleration by = new Acceleration(biasY,
15203                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15204         final Acceleration bz = new Acceleration(biasZ,
15205                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15206 
15207         final Matrix ma = generateMaCommonAxis();
15208         final double sx = ma.getElementAt(0, 0);
15209         final double sy = ma.getElementAt(1, 1);
15210         final double sz = ma.getElementAt(2, 2);
15211         final double mxy = ma.getElementAt(0, 1);
15212         final double mxz = ma.getElementAt(0, 2);
15213         final double myx = ma.getElementAt(1, 0);
15214         final double myz = ma.getElementAt(1, 2);
15215         final double mzx = ma.getElementAt(2, 0);
15216         final double mzy = ma.getElementAt(2, 1);
15217 
15218         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15219         final double latitude = Math.toRadians(
15220                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15221         final double longitude = Math.toRadians(
15222                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15223         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15224         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15225         final NEDVelocity nedVelocity = new NEDVelocity();
15226         final ECEFPosition ecefPosition = new ECEFPosition();
15227         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15228         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15229                 ecefPosition, ecefVelocity);
15230 
15231         final KnownPositionAccelerometerCalibrator calibrator =
15232                 new KnownPositionAccelerometerCalibrator(ecefPosition,
15233                         bx, by, bz, sx, sy, sz, mxy, mxz,
15234                         myx, myz, mzx, mzy);
15235 
15236         // check default values
15237         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15238         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15239         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15240         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15241         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15242         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15243         final Acceleration bx2 = new Acceleration(0.0,
15244                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15245         calibrator.getInitialBiasXAsAcceleration(bx2);
15246         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15247         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15248         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15249         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15250         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15251         final Acceleration by2 = new Acceleration(0.0,
15252                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15253         calibrator.getInitialBiasYAsAcceleration(by2);
15254         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15255         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15256         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15257         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15258         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15259         final Acceleration bz2 = new Acceleration(0.0,
15260                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15261         calibrator.getInitialBiasZAsAcceleration(bz2);
15262         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15263         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15264         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15265         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15266         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15267         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15268         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15269         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15270         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15271         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15272         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15273         final double[] bias1 = calibrator.getInitialBias();
15274         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15275         final double[] bias2 = new double[3];
15276         calibrator.getInitialBias(bias2);
15277         assertArrayEquals(bias1, bias2, 0.0);
15278         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15279         assertEquals(b1, ba);
15280         final Matrix b2 = new Matrix(3, 1);
15281         calibrator.getInitialBiasAsMatrix(b2);
15282         assertEquals(b1, b2);
15283         final Matrix ma1 = new Matrix(3, 3);
15284         ma1.setSubmatrix(0, 0,
15285                 2, 2,
15286                 new double[]{sx, myx, mzx,
15287                         mxy, sy, mzy,
15288                         mxz, myz, sz});
15289         assertEquals(calibrator.getInitialMa(), ma1);
15290         final Matrix ma2 = new Matrix(3, 3);
15291         calibrator.getInitialMa(ma2);
15292         assertEquals(ma1, ma2);
15293         assertSame(calibrator.getEcefPosition(), ecefPosition);
15294         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15295         final NEDPosition nedPosition1 = new NEDPosition();
15296         assertTrue(calibrator.getNedPosition(nedPosition1));
15297         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15298         assertNull(calibrator.getMeasurements());
15299         assertFalse(calibrator.isCommonAxisUsed());
15300         assertNull(calibrator.getListener());
15301         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15302         assertFalse(calibrator.isReady());
15303         assertFalse(calibrator.isRunning());
15304         assertNull(calibrator.getEstimatedBiases());
15305         assertFalse(calibrator.getEstimatedBiases(null));
15306         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15307         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15308         assertNull(calibrator.getEstimatedBiasFx());
15309         assertNull(calibrator.getEstimatedBiasFy());
15310         assertNull(calibrator.getEstimatedBiasFz());
15311         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15312         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15313         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15314         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15315         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15316         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15317         assertNull(calibrator.getEstimatedMa());
15318         assertNull(calibrator.getEstimatedSx());
15319         assertNull(calibrator.getEstimatedSy());
15320         assertNull(calibrator.getEstimatedSz());
15321         assertNull(calibrator.getEstimatedMxy());
15322         assertNull(calibrator.getEstimatedMxz());
15323         assertNull(calibrator.getEstimatedMyx());
15324         assertNull(calibrator.getEstimatedMyz());
15325         assertNull(calibrator.getEstimatedMzx());
15326         assertNull(calibrator.getEstimatedMzy());
15327         assertNull(calibrator.getEstimatedCovariance());
15328         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15329         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15330                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15331         assertNotNull(calibrator.getGroundTruthGravityNorm());
15332         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15333         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15334         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15335                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15336         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15337         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15338         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15339     }
15340 
15341     @Test
15342     public void testConstructor126() throws WrongSizeException {
15343         final Matrix ba = generateBa();
15344         final double biasX = ba.getElementAtIndex(0);
15345         final double biasY = ba.getElementAtIndex(1);
15346         final double biasZ = ba.getElementAtIndex(2);
15347 
15348         final Acceleration bx = new Acceleration(biasX,
15349                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15350         final Acceleration by = new Acceleration(biasY,
15351                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15352         final Acceleration bz = new Acceleration(biasZ,
15353                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15354 
15355         final Matrix ma = generateMaCommonAxis();
15356         final double sx = ma.getElementAt(0, 0);
15357         final double sy = ma.getElementAt(1, 1);
15358         final double sz = ma.getElementAt(2, 2);
15359         final double mxy = ma.getElementAt(0, 1);
15360         final double mxz = ma.getElementAt(0, 2);
15361         final double myx = ma.getElementAt(1, 0);
15362         final double myz = ma.getElementAt(1, 2);
15363         final double mzx = ma.getElementAt(2, 0);
15364         final double mzy = ma.getElementAt(2, 1);
15365 
15366         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15367         final double latitude = Math.toRadians(
15368                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15369         final double longitude = Math.toRadians(
15370                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15371         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15372         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15373         final NEDVelocity nedVelocity = new NEDVelocity();
15374         final ECEFPosition ecefPosition = new ECEFPosition();
15375         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15376         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15377                 ecefPosition, ecefVelocity);
15378 
15379         final KnownPositionAccelerometerCalibrator calibrator =
15380                 new KnownPositionAccelerometerCalibrator(ecefPosition,
15381                         bx, by, bz, sx, sy, sz, mxy, mxz,
15382                         myx, myz, mzx, mzy, this);
15383 
15384         // check default values
15385         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15386         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15387         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15388         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15389         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15390         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15391         final Acceleration bx2 = new Acceleration(0.0,
15392                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15393         calibrator.getInitialBiasXAsAcceleration(bx2);
15394         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15395         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15396         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15397         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15398         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15399         final Acceleration by2 = new Acceleration(0.0,
15400                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15401         calibrator.getInitialBiasYAsAcceleration(by2);
15402         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15403         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15404         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15405         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15406         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15407         final Acceleration bz2 = new Acceleration(0.0,
15408                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15409         calibrator.getInitialBiasZAsAcceleration(bz2);
15410         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15411         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15412         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15413         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15414         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15415         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15416         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15417         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15418         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15419         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15420         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15421         final double[] bias1 = calibrator.getInitialBias();
15422         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15423         final double[] bias2 = new double[3];
15424         calibrator.getInitialBias(bias2);
15425         assertArrayEquals(bias1, bias2, 0.0);
15426         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15427         assertEquals(b1, ba);
15428         final Matrix b2 = new Matrix(3, 1);
15429         calibrator.getInitialBiasAsMatrix(b2);
15430         assertEquals(b1, b2);
15431         final Matrix ma1 = new Matrix(3, 3);
15432         ma1.setSubmatrix(0, 0,
15433                 2, 2,
15434                 new double[]{sx, myx, mzx,
15435                         mxy, sy, mzy,
15436                         mxz, myz, sz});
15437         assertEquals(calibrator.getInitialMa(), ma1);
15438         final Matrix ma2 = new Matrix(3, 3);
15439         calibrator.getInitialMa(ma2);
15440         assertEquals(ma1, ma2);
15441         assertSame(calibrator.getEcefPosition(), ecefPosition);
15442         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15443         final NEDPosition nedPosition1 = new NEDPosition();
15444         assertTrue(calibrator.getNedPosition(nedPosition1));
15445         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15446         assertNull(calibrator.getMeasurements());
15447         assertFalse(calibrator.isCommonAxisUsed());
15448         assertSame(calibrator.getListener(), this);
15449         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15450         assertFalse(calibrator.isReady());
15451         assertFalse(calibrator.isRunning());
15452         assertNull(calibrator.getEstimatedBiases());
15453         assertFalse(calibrator.getEstimatedBiases(null));
15454         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15455         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15456         assertNull(calibrator.getEstimatedBiasFx());
15457         assertNull(calibrator.getEstimatedBiasFy());
15458         assertNull(calibrator.getEstimatedBiasFz());
15459         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15460         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15461         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15462         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15463         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15464         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15465         assertNull(calibrator.getEstimatedMa());
15466         assertNull(calibrator.getEstimatedSx());
15467         assertNull(calibrator.getEstimatedSy());
15468         assertNull(calibrator.getEstimatedSz());
15469         assertNull(calibrator.getEstimatedMxy());
15470         assertNull(calibrator.getEstimatedMxz());
15471         assertNull(calibrator.getEstimatedMyx());
15472         assertNull(calibrator.getEstimatedMyz());
15473         assertNull(calibrator.getEstimatedMzx());
15474         assertNull(calibrator.getEstimatedMzy());
15475         assertNull(calibrator.getEstimatedCovariance());
15476         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15477         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15478                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15479         assertNotNull(calibrator.getGroundTruthGravityNorm());
15480         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15481         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15482         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15483                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15484         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15485         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15486         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15487     }
15488 
15489     @Test
15490     public void testConstructor127() throws WrongSizeException {
15491         final Collection<StandardDeviationBodyKinematics> measurements =
15492                 Collections.emptyList();
15493 
15494         final Matrix ba = generateBa();
15495         final double biasX = ba.getElementAtIndex(0);
15496         final double biasY = ba.getElementAtIndex(1);
15497         final double biasZ = ba.getElementAtIndex(2);
15498 
15499         final Acceleration bx = new Acceleration(biasX,
15500                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15501         final Acceleration by = new Acceleration(biasY,
15502                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15503         final Acceleration bz = new Acceleration(biasZ,
15504                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15505 
15506         final Matrix ma = generateMaCommonAxis();
15507         final double sx = ma.getElementAt(0, 0);
15508         final double sy = ma.getElementAt(1, 1);
15509         final double sz = ma.getElementAt(2, 2);
15510         final double mxy = ma.getElementAt(0, 1);
15511         final double mxz = ma.getElementAt(0, 2);
15512         final double myx = ma.getElementAt(1, 0);
15513         final double myz = ma.getElementAt(1, 2);
15514         final double mzx = ma.getElementAt(2, 0);
15515         final double mzy = ma.getElementAt(2, 1);
15516 
15517         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15518         final double latitude = Math.toRadians(
15519                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15520         final double longitude = Math.toRadians(
15521                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15522         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15523         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15524         final NEDVelocity nedVelocity = new NEDVelocity();
15525         final ECEFPosition ecefPosition = new ECEFPosition();
15526         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15527         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15528                 ecefPosition, ecefVelocity);
15529 
15530         final KnownPositionAccelerometerCalibrator calibrator =
15531                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15532                         bx, by, bz, sx, sy, sz, mxy, mxz,
15533                         myx, myz, mzx, mzy);
15534 
15535         // check default values
15536         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15537         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15538         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15539         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15540         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15541         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15542         final Acceleration bx2 = new Acceleration(0.0,
15543                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15544         calibrator.getInitialBiasXAsAcceleration(bx2);
15545         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15546         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15547         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15548         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15549         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15550         final Acceleration by2 = new Acceleration(0.0,
15551                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15552         calibrator.getInitialBiasYAsAcceleration(by2);
15553         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15554         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15555         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15556         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15557         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15558         final Acceleration bz2 = new Acceleration(0.0,
15559                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15560         calibrator.getInitialBiasZAsAcceleration(bz2);
15561         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15562         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15563         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15564         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15565         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15566         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15567         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15568         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15569         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15570         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15571         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15572         final double[] bias1 = calibrator.getInitialBias();
15573         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15574         final double[] bias2 = new double[3];
15575         calibrator.getInitialBias(bias2);
15576         assertArrayEquals(bias1, bias2, 0.0);
15577         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15578         assertEquals(b1, ba);
15579         final Matrix b2 = new Matrix(3, 1);
15580         calibrator.getInitialBiasAsMatrix(b2);
15581         assertEquals(b1, b2);
15582         final Matrix ma1 = new Matrix(3, 3);
15583         ma1.setSubmatrix(0, 0,
15584                 2, 2,
15585                 new double[]{sx, myx, mzx,
15586                         mxy, sy, mzy,
15587                         mxz, myz, sz});
15588         assertEquals(calibrator.getInitialMa(), ma1);
15589         final Matrix ma2 = new Matrix(3, 3);
15590         calibrator.getInitialMa(ma2);
15591         assertEquals(ma1, ma2);
15592         assertSame(calibrator.getEcefPosition(), ecefPosition);
15593         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15594         final NEDPosition nedPosition1 = new NEDPosition();
15595         assertTrue(calibrator.getNedPosition(nedPosition1));
15596         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15597         assertSame(calibrator.getMeasurements(), measurements);
15598         assertFalse(calibrator.isCommonAxisUsed());
15599         assertNull(calibrator.getListener());
15600         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15601         assertFalse(calibrator.isReady());
15602         assertFalse(calibrator.isRunning());
15603         assertNull(calibrator.getEstimatedBiases());
15604         assertFalse(calibrator.getEstimatedBiases(null));
15605         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15606         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15607         assertNull(calibrator.getEstimatedBiasFx());
15608         assertNull(calibrator.getEstimatedBiasFy());
15609         assertNull(calibrator.getEstimatedBiasFz());
15610         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15611         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15612         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15613         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15614         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15615         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15616         assertNull(calibrator.getEstimatedMa());
15617         assertNull(calibrator.getEstimatedSx());
15618         assertNull(calibrator.getEstimatedSy());
15619         assertNull(calibrator.getEstimatedSz());
15620         assertNull(calibrator.getEstimatedMxy());
15621         assertNull(calibrator.getEstimatedMxz());
15622         assertNull(calibrator.getEstimatedMyx());
15623         assertNull(calibrator.getEstimatedMyz());
15624         assertNull(calibrator.getEstimatedMzx());
15625         assertNull(calibrator.getEstimatedMzy());
15626         assertNull(calibrator.getEstimatedCovariance());
15627         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15628         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15629                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15630         assertNotNull(calibrator.getGroundTruthGravityNorm());
15631         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15632         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15633         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15634                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15635         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15636         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15637         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15638     }
15639 
15640     @Test
15641     public void testConstructor128() throws WrongSizeException {
15642         final Collection<StandardDeviationBodyKinematics> measurements =
15643                 Collections.emptyList();
15644 
15645         final Matrix ba = generateBa();
15646         final double biasX = ba.getElementAtIndex(0);
15647         final double biasY = ba.getElementAtIndex(1);
15648         final double biasZ = ba.getElementAtIndex(2);
15649 
15650         final Acceleration bx = new Acceleration(biasX,
15651                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15652         final Acceleration by = new Acceleration(biasY,
15653                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15654         final Acceleration bz = new Acceleration(biasZ,
15655                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15656 
15657         final Matrix ma = generateMaCommonAxis();
15658         final double sx = ma.getElementAt(0, 0);
15659         final double sy = ma.getElementAt(1, 1);
15660         final double sz = ma.getElementAt(2, 2);
15661         final double mxy = ma.getElementAt(0, 1);
15662         final double mxz = ma.getElementAt(0, 2);
15663         final double myx = ma.getElementAt(1, 0);
15664         final double myz = ma.getElementAt(1, 2);
15665         final double mzx = ma.getElementAt(2, 0);
15666         final double mzy = ma.getElementAt(2, 1);
15667 
15668         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15669         final double latitude = Math.toRadians(
15670                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15671         final double longitude = Math.toRadians(
15672                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15673         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15674         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15675         final NEDVelocity nedVelocity = new NEDVelocity();
15676         final ECEFPosition ecefPosition = new ECEFPosition();
15677         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15678         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15679                 ecefPosition, ecefVelocity);
15680 
15681         final KnownPositionAccelerometerCalibrator calibrator =
15682                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15683                         bx, by, bz, sx, sy, sz, mxy, mxz,
15684                         myx, myz, mzx, mzy, this);
15685 
15686         // check default values
15687         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15688         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15689         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15690         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15691         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15692         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15693         final Acceleration bx2 = new Acceleration(0.0,
15694                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15695         calibrator.getInitialBiasXAsAcceleration(bx2);
15696         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15697         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15698         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15699         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15700         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15701         final Acceleration by2 = new Acceleration(0.0,
15702                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15703         calibrator.getInitialBiasYAsAcceleration(by2);
15704         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15705         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15706         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15707         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15708         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15709         final Acceleration bz2 = new Acceleration(0.0,
15710                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15711         calibrator.getInitialBiasZAsAcceleration(bz2);
15712         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15713         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15714         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15715         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15716         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15717         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15718         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15719         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15720         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15721         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15722         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15723         final double[] bias1 = calibrator.getInitialBias();
15724         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15725         final double[] bias2 = new double[3];
15726         calibrator.getInitialBias(bias2);
15727         assertArrayEquals(bias1, bias2, 0.0);
15728         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15729         assertEquals(b1, ba);
15730         final Matrix b2 = new Matrix(3, 1);
15731         calibrator.getInitialBiasAsMatrix(b2);
15732         assertEquals(b1, b2);
15733         final Matrix ma1 = new Matrix(3, 3);
15734         ma1.setSubmatrix(0, 0,
15735                 2, 2,
15736                 new double[]{sx, myx, mzx,
15737                         mxy, sy, mzy,
15738                         mxz, myz, sz});
15739         assertEquals(calibrator.getInitialMa(), ma1);
15740         final Matrix ma2 = new Matrix(3, 3);
15741         calibrator.getInitialMa(ma2);
15742         assertEquals(ma1, ma2);
15743         assertSame(calibrator.getEcefPosition(), ecefPosition);
15744         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15745         final NEDPosition nedPosition1 = new NEDPosition();
15746         assertTrue(calibrator.getNedPosition(nedPosition1));
15747         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15748         assertSame(calibrator.getMeasurements(), measurements);
15749         assertFalse(calibrator.isCommonAxisUsed());
15750         assertSame(calibrator.getListener(), this);
15751         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15752         assertFalse(calibrator.isReady());
15753         assertFalse(calibrator.isRunning());
15754         assertNull(calibrator.getEstimatedBiases());
15755         assertFalse(calibrator.getEstimatedBiases(null));
15756         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15757         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15758         assertNull(calibrator.getEstimatedBiasFx());
15759         assertNull(calibrator.getEstimatedBiasFy());
15760         assertNull(calibrator.getEstimatedBiasFz());
15761         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15762         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15763         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15764         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15765         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15766         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15767         assertNull(calibrator.getEstimatedMa());
15768         assertNull(calibrator.getEstimatedSx());
15769         assertNull(calibrator.getEstimatedSy());
15770         assertNull(calibrator.getEstimatedSz());
15771         assertNull(calibrator.getEstimatedMxy());
15772         assertNull(calibrator.getEstimatedMxz());
15773         assertNull(calibrator.getEstimatedMyx());
15774         assertNull(calibrator.getEstimatedMyz());
15775         assertNull(calibrator.getEstimatedMzx());
15776         assertNull(calibrator.getEstimatedMzy());
15777         assertNull(calibrator.getEstimatedCovariance());
15778         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15779         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15780                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15781         assertNotNull(calibrator.getGroundTruthGravityNorm());
15782         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15783         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15784         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15785                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15786         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15787         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15788         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15789     }
15790 
15791     @Test
15792     public void testConstructor129() throws WrongSizeException {
15793         final Matrix ba = generateBa();
15794         final double biasX = ba.getElementAtIndex(0);
15795         final double biasY = ba.getElementAtIndex(1);
15796         final double biasZ = ba.getElementAtIndex(2);
15797 
15798         final Acceleration bx = new Acceleration(biasX,
15799                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15800         final Acceleration by = new Acceleration(biasY,
15801                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15802         final Acceleration bz = new Acceleration(biasZ,
15803                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15804 
15805         final Matrix ma = generateMaCommonAxis();
15806         final double sx = ma.getElementAt(0, 0);
15807         final double sy = ma.getElementAt(1, 1);
15808         final double sz = ma.getElementAt(2, 2);
15809         final double mxy = ma.getElementAt(0, 1);
15810         final double mxz = ma.getElementAt(0, 2);
15811         final double myx = ma.getElementAt(1, 0);
15812         final double myz = ma.getElementAt(1, 2);
15813         final double mzx = ma.getElementAt(2, 0);
15814         final double mzy = ma.getElementAt(2, 1);
15815 
15816         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15817         final double latitude = Math.toRadians(
15818                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15819         final double longitude = Math.toRadians(
15820                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15821         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15822         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15823         final NEDVelocity nedVelocity = new NEDVelocity();
15824         final ECEFPosition ecefPosition = new ECEFPosition();
15825         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15826         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15827                 ecefPosition, ecefVelocity);
15828 
15829         final KnownPositionAccelerometerCalibrator calibrator =
15830                 new KnownPositionAccelerometerCalibrator(ecefPosition,
15831                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
15832                         myx, myz, mzx, mzy);
15833 
15834         // check default values
15835         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15836         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15837         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15838         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15839         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15840         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15841         final Acceleration bx2 = new Acceleration(0.0,
15842                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15843         calibrator.getInitialBiasXAsAcceleration(bx2);
15844         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15845         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15846         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15847         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15848         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15849         final Acceleration by2 = new Acceleration(0.0,
15850                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15851         calibrator.getInitialBiasYAsAcceleration(by2);
15852         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15853         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15854         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15855         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15856         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15857         final Acceleration bz2 = new Acceleration(0.0,
15858                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15859         calibrator.getInitialBiasZAsAcceleration(bz2);
15860         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15861         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15862         assertEquals(calibrator.getInitialSx(), sx, 0.0);
15863         assertEquals(calibrator.getInitialSy(), sy, 0.0);
15864         assertEquals(calibrator.getInitialSz(), sz, 0.0);
15865         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15866         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15867         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15868         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15869         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15870         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15871         final double[] bias1 = calibrator.getInitialBias();
15872         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15873         final double[] bias2 = new double[3];
15874         calibrator.getInitialBias(bias2);
15875         assertArrayEquals(bias1, bias2, 0.0);
15876         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15877         assertEquals(b1, ba);
15878         final Matrix b2 = new Matrix(3, 1);
15879         calibrator.getInitialBiasAsMatrix(b2);
15880         assertEquals(b1, b2);
15881         final Matrix ma1 = new Matrix(3, 3);
15882         ma1.setSubmatrix(0, 0,
15883                 2, 2,
15884                 new double[]{sx, myx, mzx,
15885                         mxy, sy, mzy,
15886                         mxz, myz, sz});
15887         assertEquals(calibrator.getInitialMa(), ma1);
15888         final Matrix ma2 = new Matrix(3, 3);
15889         calibrator.getInitialMa(ma2);
15890         assertEquals(ma1, ma2);
15891         assertSame(calibrator.getEcefPosition(), ecefPosition);
15892         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15893         final NEDPosition nedPosition1 = new NEDPosition();
15894         assertTrue(calibrator.getNedPosition(nedPosition1));
15895         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15896         assertNull(calibrator.getMeasurements());
15897         assertTrue(calibrator.isCommonAxisUsed());
15898         assertNull(calibrator.getListener());
15899         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15900         assertFalse(calibrator.isReady());
15901         assertFalse(calibrator.isRunning());
15902         assertNull(calibrator.getEstimatedBiases());
15903         assertFalse(calibrator.getEstimatedBiases(null));
15904         assertNull(calibrator.getEstimatedBiasesAsMatrix());
15905         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15906         assertNull(calibrator.getEstimatedBiasFx());
15907         assertNull(calibrator.getEstimatedBiasFy());
15908         assertNull(calibrator.getEstimatedBiasFz());
15909         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15910         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15911         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15912         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15913         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15914         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15915         assertNull(calibrator.getEstimatedMa());
15916         assertNull(calibrator.getEstimatedSx());
15917         assertNull(calibrator.getEstimatedSy());
15918         assertNull(calibrator.getEstimatedSz());
15919         assertNull(calibrator.getEstimatedMxy());
15920         assertNull(calibrator.getEstimatedMxz());
15921         assertNull(calibrator.getEstimatedMyx());
15922         assertNull(calibrator.getEstimatedMyz());
15923         assertNull(calibrator.getEstimatedMzx());
15924         assertNull(calibrator.getEstimatedMzy());
15925         assertNull(calibrator.getEstimatedCovariance());
15926         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15927         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15928                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15929         assertNotNull(calibrator.getGroundTruthGravityNorm());
15930         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15931         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15932         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15933                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15934         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15935         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15936         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15937     }
15938 
15939     @Test
15940     public void testConstructor130() throws WrongSizeException {
15941         final Matrix ba = generateBa();
15942         final double biasX = ba.getElementAtIndex(0);
15943         final double biasY = ba.getElementAtIndex(1);
15944         final double biasZ = ba.getElementAtIndex(2);
15945 
15946         final Acceleration bx = new Acceleration(biasX,
15947                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15948         final Acceleration by = new Acceleration(biasY,
15949                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15950         final Acceleration bz = new Acceleration(biasZ,
15951                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15952 
15953         final Matrix ma = generateMaCommonAxis();
15954         final double sx = ma.getElementAt(0, 0);
15955         final double sy = ma.getElementAt(1, 1);
15956         final double sz = ma.getElementAt(2, 2);
15957         final double mxy = ma.getElementAt(0, 1);
15958         final double mxz = ma.getElementAt(0, 2);
15959         final double myx = ma.getElementAt(1, 0);
15960         final double myz = ma.getElementAt(1, 2);
15961         final double mzx = ma.getElementAt(2, 0);
15962         final double mzy = ma.getElementAt(2, 1);
15963 
15964         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15965         final double latitude = Math.toRadians(
15966                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15967         final double longitude = Math.toRadians(
15968                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15969         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15970         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15971         final NEDVelocity nedVelocity = new NEDVelocity();
15972         final ECEFPosition ecefPosition = new ECEFPosition();
15973         final ECEFVelocity ecefVelocity = new ECEFVelocity();
15974         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15975                 ecefPosition, ecefVelocity);
15976 
15977         final KnownPositionAccelerometerCalibrator calibrator =
15978                 new KnownPositionAccelerometerCalibrator(ecefPosition,
15979                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
15980                         myx, myz, mzx, mzy, this);
15981 
15982         // check default values
15983         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15984         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15985         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15986         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15987         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15988         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15989         final Acceleration bx2 = new Acceleration(0.0,
15990                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15991         calibrator.getInitialBiasXAsAcceleration(bx2);
15992         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15993         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15994         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15995         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15996         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15997         final Acceleration by2 = new Acceleration(0.0,
15998                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15999         calibrator.getInitialBiasYAsAcceleration(by2);
16000         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16001         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16002         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16003         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16004         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16005         final Acceleration bz2 = new Acceleration(0.0,
16006                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16007         calibrator.getInitialBiasZAsAcceleration(bz2);
16008         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16009         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16010         assertEquals(calibrator.getInitialSx(), sx, 0.0);
16011         assertEquals(calibrator.getInitialSy(), sy, 0.0);
16012         assertEquals(calibrator.getInitialSz(), sz, 0.0);
16013         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16014         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16015         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16016         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16017         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16018         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16019         final double[] bias1 = calibrator.getInitialBias();
16020         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16021         final double[] bias2 = new double[3];
16022         calibrator.getInitialBias(bias2);
16023         assertArrayEquals(bias1, bias2, 0.0);
16024         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16025         assertEquals(b1, ba);
16026         final Matrix b2 = new Matrix(3, 1);
16027         calibrator.getInitialBiasAsMatrix(b2);
16028         assertEquals(b1, b2);
16029         final Matrix ma1 = new Matrix(3, 3);
16030         ma1.setSubmatrix(0, 0,
16031                 2, 2,
16032                 new double[]{sx, myx, mzx,
16033                         mxy, sy, mzy,
16034                         mxz, myz, sz});
16035         assertEquals(calibrator.getInitialMa(), ma1);
16036         final Matrix ma2 = new Matrix(3, 3);
16037         calibrator.getInitialMa(ma2);
16038         assertEquals(ma1, ma2);
16039         assertSame(calibrator.getEcefPosition(), ecefPosition);
16040         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16041         final NEDPosition nedPosition1 = new NEDPosition();
16042         assertTrue(calibrator.getNedPosition(nedPosition1));
16043         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16044         assertNull(calibrator.getMeasurements());
16045         assertTrue(calibrator.isCommonAxisUsed());
16046         assertSame(calibrator.getListener(), this);
16047         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16048         assertFalse(calibrator.isReady());
16049         assertFalse(calibrator.isRunning());
16050         assertNull(calibrator.getEstimatedBiases());
16051         assertFalse(calibrator.getEstimatedBiases(null));
16052         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16053         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16054         assertNull(calibrator.getEstimatedBiasFx());
16055         assertNull(calibrator.getEstimatedBiasFy());
16056         assertNull(calibrator.getEstimatedBiasFz());
16057         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16058         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16059         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16060         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16061         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16062         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16063         assertNull(calibrator.getEstimatedMa());
16064         assertNull(calibrator.getEstimatedSx());
16065         assertNull(calibrator.getEstimatedSy());
16066         assertNull(calibrator.getEstimatedSz());
16067         assertNull(calibrator.getEstimatedMxy());
16068         assertNull(calibrator.getEstimatedMxz());
16069         assertNull(calibrator.getEstimatedMyx());
16070         assertNull(calibrator.getEstimatedMyz());
16071         assertNull(calibrator.getEstimatedMzx());
16072         assertNull(calibrator.getEstimatedMzy());
16073         assertNull(calibrator.getEstimatedCovariance());
16074         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16075         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16076                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16077         assertNotNull(calibrator.getGroundTruthGravityNorm());
16078         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16079         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16080         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16081                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16082         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16083         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16084         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16085     }
16086 
16087     @Test
16088     public void testConstructor131() throws WrongSizeException {
16089         final Collection<StandardDeviationBodyKinematics> measurements =
16090                 Collections.emptyList();
16091 
16092         final Matrix ba = generateBa();
16093         final double biasX = ba.getElementAtIndex(0);
16094         final double biasY = ba.getElementAtIndex(1);
16095         final double biasZ = ba.getElementAtIndex(2);
16096 
16097         final Acceleration bx = new Acceleration(biasX,
16098                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16099         final Acceleration by = new Acceleration(biasY,
16100                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16101         final Acceleration bz = new Acceleration(biasZ,
16102                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16103 
16104         final Matrix ma = generateMaCommonAxis();
16105         final double sx = ma.getElementAt(0, 0);
16106         final double sy = ma.getElementAt(1, 1);
16107         final double sz = ma.getElementAt(2, 2);
16108         final double mxy = ma.getElementAt(0, 1);
16109         final double mxz = ma.getElementAt(0, 2);
16110         final double myx = ma.getElementAt(1, 0);
16111         final double myz = ma.getElementAt(1, 2);
16112         final double mzx = ma.getElementAt(2, 0);
16113         final double mzy = ma.getElementAt(2, 1);
16114 
16115         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16116         final double latitude = Math.toRadians(
16117                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16118         final double longitude = Math.toRadians(
16119                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16120         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16121         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16122         final NEDVelocity nedVelocity = new NEDVelocity();
16123         final ECEFPosition ecefPosition = new ECEFPosition();
16124         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16125         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16126                 ecefPosition, ecefVelocity);
16127 
16128         final KnownPositionAccelerometerCalibrator calibrator =
16129                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16130                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
16131                         myx, myz, mzx, mzy);
16132 
16133         // check default values
16134         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16135         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16136         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16137         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16138         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16139         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16140         final Acceleration bx2 = new Acceleration(0.0,
16141                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16142         calibrator.getInitialBiasXAsAcceleration(bx2);
16143         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16144         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16145         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16146         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16147         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16148         final Acceleration by2 = new Acceleration(0.0,
16149                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16150         calibrator.getInitialBiasYAsAcceleration(by2);
16151         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16152         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16153         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16154         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16155         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16156         final Acceleration bz2 = new Acceleration(0.0,
16157                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16158         calibrator.getInitialBiasZAsAcceleration(bz2);
16159         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16160         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16161         assertEquals(calibrator.getInitialSx(), sx, 0.0);
16162         assertEquals(calibrator.getInitialSy(), sy, 0.0);
16163         assertEquals(calibrator.getInitialSz(), sz, 0.0);
16164         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16165         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16166         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16167         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16168         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16169         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16170         final double[] bias1 = calibrator.getInitialBias();
16171         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16172         final double[] bias2 = new double[3];
16173         calibrator.getInitialBias(bias2);
16174         assertArrayEquals(bias1, bias2, 0.0);
16175         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16176         assertEquals(b1, ba);
16177         final Matrix b2 = new Matrix(3, 1);
16178         calibrator.getInitialBiasAsMatrix(b2);
16179         assertEquals(b1, b2);
16180         final Matrix ma1 = new Matrix(3, 3);
16181         ma1.setSubmatrix(0, 0,
16182                 2, 2,
16183                 new double[]{sx, myx, mzx,
16184                         mxy, sy, mzy,
16185                         mxz, myz, sz});
16186         assertEquals(calibrator.getInitialMa(), ma1);
16187         final Matrix ma2 = new Matrix(3, 3);
16188         calibrator.getInitialMa(ma2);
16189         assertEquals(ma1, ma2);
16190         assertSame(calibrator.getEcefPosition(), ecefPosition);
16191         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16192         final NEDPosition nedPosition1 = new NEDPosition();
16193         assertTrue(calibrator.getNedPosition(nedPosition1));
16194         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16195         assertSame(calibrator.getMeasurements(), measurements);
16196         assertTrue(calibrator.isCommonAxisUsed());
16197         assertNull(calibrator.getListener());
16198         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16199         assertFalse(calibrator.isReady());
16200         assertFalse(calibrator.isRunning());
16201         assertNull(calibrator.getEstimatedBiases());
16202         assertFalse(calibrator.getEstimatedBiases(null));
16203         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16204         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16205         assertNull(calibrator.getEstimatedBiasFx());
16206         assertNull(calibrator.getEstimatedBiasFy());
16207         assertNull(calibrator.getEstimatedBiasFz());
16208         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16209         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16210         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16211         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16212         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16213         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16214         assertNull(calibrator.getEstimatedMa());
16215         assertNull(calibrator.getEstimatedSx());
16216         assertNull(calibrator.getEstimatedSy());
16217         assertNull(calibrator.getEstimatedSz());
16218         assertNull(calibrator.getEstimatedMxy());
16219         assertNull(calibrator.getEstimatedMxz());
16220         assertNull(calibrator.getEstimatedMyx());
16221         assertNull(calibrator.getEstimatedMyz());
16222         assertNull(calibrator.getEstimatedMzx());
16223         assertNull(calibrator.getEstimatedMzy());
16224         assertNull(calibrator.getEstimatedCovariance());
16225         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16226         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16227                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16228         assertNotNull(calibrator.getGroundTruthGravityNorm());
16229         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16230         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16231         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16232                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16233         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16234         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16235         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16236     }
16237 
16238     @Test
16239     public void testConstructor132() throws WrongSizeException {
16240         final Collection<StandardDeviationBodyKinematics> measurements =
16241                 Collections.emptyList();
16242 
16243         final Matrix ba = generateBa();
16244         final double biasX = ba.getElementAtIndex(0);
16245         final double biasY = ba.getElementAtIndex(1);
16246         final double biasZ = ba.getElementAtIndex(2);
16247 
16248         final Acceleration bx = new Acceleration(biasX,
16249                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16250         final Acceleration by = new Acceleration(biasY,
16251                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16252         final Acceleration bz = new Acceleration(biasZ,
16253                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16254 
16255         final Matrix ma = generateMaCommonAxis();
16256         final double sx = ma.getElementAt(0, 0);
16257         final double sy = ma.getElementAt(1, 1);
16258         final double sz = ma.getElementAt(2, 2);
16259         final double mxy = ma.getElementAt(0, 1);
16260         final double mxz = ma.getElementAt(0, 2);
16261         final double myx = ma.getElementAt(1, 0);
16262         final double myz = ma.getElementAt(1, 2);
16263         final double mzx = ma.getElementAt(2, 0);
16264         final double mzy = ma.getElementAt(2, 1);
16265 
16266         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16267         final double latitude = Math.toRadians(
16268                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16269         final double longitude = Math.toRadians(
16270                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16271         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16272         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16273         final NEDVelocity nedVelocity = new NEDVelocity();
16274         final ECEFPosition ecefPosition = new ECEFPosition();
16275         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16276         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16277                 ecefPosition, ecefVelocity);
16278 
16279         final KnownPositionAccelerometerCalibrator calibrator =
16280                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16281                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
16282                         myx, myz, mzx, mzy, this);
16283 
16284         // check default values
16285         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16286         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16287         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16288         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16289         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16290         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16291         final Acceleration bx2 = new Acceleration(0.0,
16292                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16293         calibrator.getInitialBiasXAsAcceleration(bx2);
16294         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16295         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16296         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16297         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16298         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16299         final Acceleration by2 = new Acceleration(0.0,
16300                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16301         calibrator.getInitialBiasYAsAcceleration(by2);
16302         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16303         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16304         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16305         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16306         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16307         final Acceleration bz2 = new Acceleration(0.0,
16308                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16309         calibrator.getInitialBiasZAsAcceleration(bz2);
16310         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16311         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16312         assertEquals(calibrator.getInitialSx(), sx, 0.0);
16313         assertEquals(calibrator.getInitialSy(), sy, 0.0);
16314         assertEquals(calibrator.getInitialSz(), sz, 0.0);
16315         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16316         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16317         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16318         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16319         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16320         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16321         final double[] bias1 = calibrator.getInitialBias();
16322         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16323         final double[] bias2 = new double[3];
16324         calibrator.getInitialBias(bias2);
16325         assertArrayEquals(bias1, bias2, 0.0);
16326         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16327         assertEquals(b1, ba);
16328         final Matrix b2 = new Matrix(3, 1);
16329         calibrator.getInitialBiasAsMatrix(b2);
16330         assertEquals(b1, b2);
16331         final Matrix ma1 = new Matrix(3, 3);
16332         ma1.setSubmatrix(0, 0,
16333                 2, 2,
16334                 new double[]{sx, myx, mzx,
16335                         mxy, sy, mzy,
16336                         mxz, myz, sz});
16337         assertEquals(calibrator.getInitialMa(), ma1);
16338         final Matrix ma2 = new Matrix(3, 3);
16339         calibrator.getInitialMa(ma2);
16340         assertEquals(ma1, ma2);
16341         assertSame(calibrator.getEcefPosition(), ecefPosition);
16342         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16343         final NEDPosition nedPosition1 = new NEDPosition();
16344         assertTrue(calibrator.getNedPosition(nedPosition1));
16345         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16346         assertSame(calibrator.getMeasurements(), measurements);
16347         assertTrue(calibrator.isCommonAxisUsed());
16348         assertSame(calibrator.getListener(), this);
16349         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16350         assertFalse(calibrator.isReady());
16351         assertFalse(calibrator.isRunning());
16352         assertNull(calibrator.getEstimatedBiases());
16353         assertFalse(calibrator.getEstimatedBiases(null));
16354         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16355         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16356         assertNull(calibrator.getEstimatedBiasFx());
16357         assertNull(calibrator.getEstimatedBiasFy());
16358         assertNull(calibrator.getEstimatedBiasFz());
16359         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16360         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16361         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16362         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16363         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16364         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16365         assertNull(calibrator.getEstimatedMa());
16366         assertNull(calibrator.getEstimatedSx());
16367         assertNull(calibrator.getEstimatedSy());
16368         assertNull(calibrator.getEstimatedSz());
16369         assertNull(calibrator.getEstimatedMxy());
16370         assertNull(calibrator.getEstimatedMxz());
16371         assertNull(calibrator.getEstimatedMyx());
16372         assertNull(calibrator.getEstimatedMyz());
16373         assertNull(calibrator.getEstimatedMzx());
16374         assertNull(calibrator.getEstimatedMzy());
16375         assertNull(calibrator.getEstimatedCovariance());
16376         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16377         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16378                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16379         assertNotNull(calibrator.getGroundTruthGravityNorm());
16380         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16381         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16382         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16383                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16384         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16385         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16386         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16387     }
16388 
16389     @Test
16390     public void testConstructor133() throws WrongSizeException {
16391         final Matrix ba = generateBa();
16392         final double[] bias = ba.getBuffer();
16393         final double biasX = ba.getElementAtIndex(0);
16394         final double biasY = ba.getElementAtIndex(1);
16395         final double biasZ = ba.getElementAtIndex(2);
16396 
16397         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16398         final double latitude = Math.toRadians(
16399                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16400         final double longitude = Math.toRadians(
16401                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16402         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16403         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16404         final NEDVelocity nedVelocity = new NEDVelocity();
16405         final ECEFPosition ecefPosition = new ECEFPosition();
16406         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16407         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16408                 ecefPosition, ecefVelocity);
16409 
16410         KnownPositionAccelerometerCalibrator calibrator =
16411                 new KnownPositionAccelerometerCalibrator(ecefPosition, bias);
16412 
16413         // check default values
16414         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16415         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16416         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16417         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16418         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16419         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16420         final Acceleration bx2 = new Acceleration(0.0,
16421                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16422         calibrator.getInitialBiasXAsAcceleration(bx2);
16423         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16424         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16425         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16426         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16427         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16428         final Acceleration by2 = new Acceleration(0.0,
16429                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16430         calibrator.getInitialBiasYAsAcceleration(by2);
16431         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16432         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16433         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16434         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16435         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16436         final Acceleration bz2 = new Acceleration(0.0,
16437                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16438         calibrator.getInitialBiasZAsAcceleration(bz2);
16439         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16440         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16441         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16442         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16443         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16444         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16445         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16446         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16447         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16448         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16449         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16450         final double[] bias1 = calibrator.getInitialBias();
16451         assertArrayEquals(bias1, bias, 0.0);
16452         final double[] bias2 = new double[3];
16453         calibrator.getInitialBias(bias2);
16454         assertArrayEquals(bias1, bias2, 0.0);
16455         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16456         assertEquals(b1, ba);
16457         final Matrix b2 = new Matrix(3, 1);
16458         calibrator.getInitialBiasAsMatrix(b2);
16459         assertEquals(b1, b2);
16460         final Matrix ma1 = calibrator.getInitialMa();
16461         assertEquals(ma1, new Matrix(3, 3));
16462         final Matrix ma2 = new Matrix(3, 3);
16463         calibrator.getInitialMa(ma2);
16464         assertEquals(ma1, ma2);
16465         assertSame(calibrator.getEcefPosition(), ecefPosition);
16466         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16467         final NEDPosition nedPosition1 = new NEDPosition();
16468         assertTrue(calibrator.getNedPosition(nedPosition1));
16469         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16470         assertNull(calibrator.getMeasurements());
16471         assertFalse(calibrator.isCommonAxisUsed());
16472         assertNull(calibrator.getListener());
16473         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16474         assertFalse(calibrator.isReady());
16475         assertFalse(calibrator.isRunning());
16476         assertNull(calibrator.getEstimatedBiases());
16477         assertFalse(calibrator.getEstimatedBiases(null));
16478         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16479         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16480         assertNull(calibrator.getEstimatedBiasFx());
16481         assertNull(calibrator.getEstimatedBiasFy());
16482         assertNull(calibrator.getEstimatedBiasFz());
16483         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16484         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16485         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16486         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16487         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16488         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16489         assertNull(calibrator.getEstimatedMa());
16490         assertNull(calibrator.getEstimatedSx());
16491         assertNull(calibrator.getEstimatedSy());
16492         assertNull(calibrator.getEstimatedSz());
16493         assertNull(calibrator.getEstimatedMxy());
16494         assertNull(calibrator.getEstimatedMxz());
16495         assertNull(calibrator.getEstimatedMyx());
16496         assertNull(calibrator.getEstimatedMyz());
16497         assertNull(calibrator.getEstimatedMzx());
16498         assertNull(calibrator.getEstimatedMzy());
16499         assertNull(calibrator.getEstimatedCovariance());
16500         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16501         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16502                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16503         assertNotNull(calibrator.getGroundTruthGravityNorm());
16504         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16505         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16506         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16507                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16508         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16509         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16510         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16511 
16512         // Force IllegalArgumentException
16513         calibrator = null;
16514         try {
16515             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16516                     new double[1]);
16517             fail("IllegalArgumentException expected but not thrown");
16518         } catch (final IllegalArgumentException ignore) {
16519         }
16520         assertNull(calibrator);
16521     }
16522 
16523     @Test
16524     public void testConstructor134() throws WrongSizeException {
16525         final Matrix ba = generateBa();
16526         final double[] bias = ba.getBuffer();
16527         final double biasX = ba.getElementAtIndex(0);
16528         final double biasY = ba.getElementAtIndex(1);
16529         final double biasZ = ba.getElementAtIndex(2);
16530 
16531         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16532         final double latitude = Math.toRadians(
16533                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16534         final double longitude = Math.toRadians(
16535                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16536         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16537         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16538         final NEDVelocity nedVelocity = new NEDVelocity();
16539         final ECEFPosition ecefPosition = new ECEFPosition();
16540         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16541         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16542                 ecefPosition, ecefVelocity);
16543 
16544         KnownPositionAccelerometerCalibrator calibrator =
16545                 new KnownPositionAccelerometerCalibrator(ecefPosition, bias,
16546                         this);
16547 
16548         // check default values
16549         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16550         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16551         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16552         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16553         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16554         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16555         final Acceleration bx2 = new Acceleration(0.0,
16556                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16557         calibrator.getInitialBiasXAsAcceleration(bx2);
16558         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16559         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16560         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16561         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16562         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16563         final Acceleration by2 = new Acceleration(0.0,
16564                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16565         calibrator.getInitialBiasYAsAcceleration(by2);
16566         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16567         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16568         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16569         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16570         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16571         final Acceleration bz2 = new Acceleration(0.0,
16572                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16573         calibrator.getInitialBiasZAsAcceleration(bz2);
16574         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16575         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16576         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16577         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16578         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16579         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16580         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16581         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16582         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16583         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16584         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16585         final double[] bias1 = calibrator.getInitialBias();
16586         assertArrayEquals(bias1, bias, 0.0);
16587         final double[] bias2 = new double[3];
16588         calibrator.getInitialBias(bias2);
16589         assertArrayEquals(bias1, bias2, 0.0);
16590         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16591         assertEquals(b1, ba);
16592         final Matrix b2 = new Matrix(3, 1);
16593         calibrator.getInitialBiasAsMatrix(b2);
16594         assertEquals(b1, b2);
16595         final Matrix ma1 = calibrator.getInitialMa();
16596         assertEquals(ma1, new Matrix(3, 3));
16597         final Matrix ma2 = new Matrix(3, 3);
16598         calibrator.getInitialMa(ma2);
16599         assertEquals(ma1, ma2);
16600         assertSame(calibrator.getEcefPosition(), ecefPosition);
16601         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16602         final NEDPosition nedPosition1 = new NEDPosition();
16603         assertTrue(calibrator.getNedPosition(nedPosition1));
16604         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16605         assertNull(calibrator.getMeasurements());
16606         assertFalse(calibrator.isCommonAxisUsed());
16607         assertSame(calibrator.getListener(), this);
16608         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16609         assertFalse(calibrator.isReady());
16610         assertFalse(calibrator.isRunning());
16611         assertNull(calibrator.getEstimatedBiases());
16612         assertFalse(calibrator.getEstimatedBiases(null));
16613         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16614         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16615         assertNull(calibrator.getEstimatedBiasFx());
16616         assertNull(calibrator.getEstimatedBiasFy());
16617         assertNull(calibrator.getEstimatedBiasFz());
16618         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16619         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16620         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16621         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16622         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16623         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16624         assertNull(calibrator.getEstimatedMa());
16625         assertNull(calibrator.getEstimatedSx());
16626         assertNull(calibrator.getEstimatedSy());
16627         assertNull(calibrator.getEstimatedSz());
16628         assertNull(calibrator.getEstimatedMxy());
16629         assertNull(calibrator.getEstimatedMxz());
16630         assertNull(calibrator.getEstimatedMyx());
16631         assertNull(calibrator.getEstimatedMyz());
16632         assertNull(calibrator.getEstimatedMzx());
16633         assertNull(calibrator.getEstimatedMzy());
16634         assertNull(calibrator.getEstimatedCovariance());
16635         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16636         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16637                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16638         assertNotNull(calibrator.getGroundTruthGravityNorm());
16639         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16640         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16641         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16642                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16643         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16644         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16645         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16646 
16647         // Force IllegalArgumentException
16648         calibrator = null;
16649         try {
16650             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16651                     new double[1], this);
16652             fail("IllegalArgumentException expected but not thrown");
16653         } catch (final IllegalArgumentException ignore) {
16654         }
16655         assertNull(calibrator);
16656     }
16657 
16658     @Test
16659     public void testConstructor135() throws WrongSizeException {
16660         final Collection<StandardDeviationBodyKinematics> measurements =
16661                 Collections.emptyList();
16662 
16663         final Matrix ba = generateBa();
16664         final double[] bias = ba.getBuffer();
16665         final double biasX = ba.getElementAtIndex(0);
16666         final double biasY = ba.getElementAtIndex(1);
16667         final double biasZ = ba.getElementAtIndex(2);
16668 
16669         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16670         final double latitude = Math.toRadians(
16671                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16672         final double longitude = Math.toRadians(
16673                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16674         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16675         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16676         final NEDVelocity nedVelocity = new NEDVelocity();
16677         final ECEFPosition ecefPosition = new ECEFPosition();
16678         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16679         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16680                 ecefPosition, ecefVelocity);
16681 
16682         KnownPositionAccelerometerCalibrator calibrator =
16683                 new KnownPositionAccelerometerCalibrator(ecefPosition,
16684                         measurements, bias);
16685 
16686         // check default values
16687         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16688         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16689         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16690         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16691         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16692         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16693         final Acceleration bx2 = new Acceleration(0.0,
16694                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16695         calibrator.getInitialBiasXAsAcceleration(bx2);
16696         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16697         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16698         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16699         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16700         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16701         final Acceleration by2 = new Acceleration(0.0,
16702                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16703         calibrator.getInitialBiasYAsAcceleration(by2);
16704         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16705         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16706         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16707         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16708         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16709         final Acceleration bz2 = new Acceleration(0.0,
16710                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16711         calibrator.getInitialBiasZAsAcceleration(bz2);
16712         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16713         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16714         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16715         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16716         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16717         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16718         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16719         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16720         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16721         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16722         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16723         final double[] bias1 = calibrator.getInitialBias();
16724         assertArrayEquals(bias1, bias, 0.0);
16725         final double[] bias2 = new double[3];
16726         calibrator.getInitialBias(bias2);
16727         assertArrayEquals(bias1, bias2, 0.0);
16728         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16729         assertEquals(b1, ba);
16730         final Matrix b2 = new Matrix(3, 1);
16731         calibrator.getInitialBiasAsMatrix(b2);
16732         assertEquals(b1, b2);
16733         final Matrix ma1 = calibrator.getInitialMa();
16734         assertEquals(ma1, new Matrix(3, 3));
16735         final Matrix ma2 = new Matrix(3, 3);
16736         calibrator.getInitialMa(ma2);
16737         assertEquals(ma1, ma2);
16738         assertSame(calibrator.getEcefPosition(), ecefPosition);
16739         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16740         final NEDPosition nedPosition1 = new NEDPosition();
16741         assertTrue(calibrator.getNedPosition(nedPosition1));
16742         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16743         assertSame(calibrator.getMeasurements(), measurements);
16744         assertFalse(calibrator.isCommonAxisUsed());
16745         assertNull(calibrator.getListener());
16746         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16747         assertFalse(calibrator.isReady());
16748         assertFalse(calibrator.isRunning());
16749         assertNull(calibrator.getEstimatedBiases());
16750         assertFalse(calibrator.getEstimatedBiases(null));
16751         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16752         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16753         assertNull(calibrator.getEstimatedBiasFx());
16754         assertNull(calibrator.getEstimatedBiasFy());
16755         assertNull(calibrator.getEstimatedBiasFz());
16756         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16757         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16758         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16759         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16760         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16761         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16762         assertNull(calibrator.getEstimatedMa());
16763         assertNull(calibrator.getEstimatedSx());
16764         assertNull(calibrator.getEstimatedSy());
16765         assertNull(calibrator.getEstimatedSz());
16766         assertNull(calibrator.getEstimatedMxy());
16767         assertNull(calibrator.getEstimatedMxz());
16768         assertNull(calibrator.getEstimatedMyx());
16769         assertNull(calibrator.getEstimatedMyz());
16770         assertNull(calibrator.getEstimatedMzx());
16771         assertNull(calibrator.getEstimatedMzy());
16772         assertNull(calibrator.getEstimatedCovariance());
16773         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16774         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16775                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16776         assertNotNull(calibrator.getGroundTruthGravityNorm());
16777         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16778         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16779         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16780                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16781         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16782         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16783         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16784 
16785         // Force IllegalArgumentException
16786         calibrator = null;
16787         try {
16788             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16789                     measurements, new double[1]);
16790             fail("IllegalArgumentException expected but not thrown");
16791         } catch (final IllegalArgumentException ignore) {
16792         }
16793         assertNull(calibrator);
16794     }
16795 
16796     @Test
16797     public void testConstructor136() throws WrongSizeException {
16798         final Collection<StandardDeviationBodyKinematics> measurements =
16799                 Collections.emptyList();
16800 
16801         final Matrix ba = generateBa();
16802         final double[] bias = ba.getBuffer();
16803         final double biasX = ba.getElementAtIndex(0);
16804         final double biasY = ba.getElementAtIndex(1);
16805         final double biasZ = ba.getElementAtIndex(2);
16806 
16807         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16808         final double latitude = Math.toRadians(
16809                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16810         final double longitude = Math.toRadians(
16811                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16812         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16813         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16814         final NEDVelocity nedVelocity = new NEDVelocity();
16815         final ECEFPosition ecefPosition = new ECEFPosition();
16816         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16817         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16818                 ecefPosition, ecefVelocity);
16819 
16820         KnownPositionAccelerometerCalibrator calibrator =
16821                 new KnownPositionAccelerometerCalibrator(ecefPosition,
16822                         measurements, bias, this);
16823 
16824         // check default values
16825         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16826         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16827         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16828         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16829         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16830         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16831         final Acceleration bx2 = new Acceleration(0.0,
16832                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16833         calibrator.getInitialBiasXAsAcceleration(bx2);
16834         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16835         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16836         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16837         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16838         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16839         final Acceleration by2 = new Acceleration(0.0,
16840                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16841         calibrator.getInitialBiasYAsAcceleration(by2);
16842         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16843         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16844         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16845         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16846         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16847         final Acceleration bz2 = new Acceleration(0.0,
16848                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16849         calibrator.getInitialBiasZAsAcceleration(bz2);
16850         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16851         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16852         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16853         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16854         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16855         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16856         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16857         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16858         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16859         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16860         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16861         final double[] bias1 = calibrator.getInitialBias();
16862         assertArrayEquals(bias1, bias, 0.0);
16863         final double[] bias2 = new double[3];
16864         calibrator.getInitialBias(bias2);
16865         assertArrayEquals(bias1, bias2, 0.0);
16866         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16867         assertEquals(b1, ba);
16868         final Matrix b2 = new Matrix(3, 1);
16869         calibrator.getInitialBiasAsMatrix(b2);
16870         assertEquals(b1, b2);
16871         final Matrix ma1 = calibrator.getInitialMa();
16872         assertEquals(ma1, new Matrix(3, 3));
16873         final Matrix ma2 = new Matrix(3, 3);
16874         calibrator.getInitialMa(ma2);
16875         assertEquals(ma1, ma2);
16876         assertSame(calibrator.getEcefPosition(), ecefPosition);
16877         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16878         final NEDPosition nedPosition1 = new NEDPosition();
16879         assertTrue(calibrator.getNedPosition(nedPosition1));
16880         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16881         assertSame(calibrator.getMeasurements(), measurements);
16882         assertFalse(calibrator.isCommonAxisUsed());
16883         assertSame(calibrator.getListener(), this);
16884         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16885         assertFalse(calibrator.isReady());
16886         assertFalse(calibrator.isRunning());
16887         assertNull(calibrator.getEstimatedBiases());
16888         assertFalse(calibrator.getEstimatedBiases(null));
16889         assertNull(calibrator.getEstimatedBiasesAsMatrix());
16890         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16891         assertNull(calibrator.getEstimatedBiasFx());
16892         assertNull(calibrator.getEstimatedBiasFy());
16893         assertNull(calibrator.getEstimatedBiasFz());
16894         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16895         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16896         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16897         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16898         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16899         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16900         assertNull(calibrator.getEstimatedMa());
16901         assertNull(calibrator.getEstimatedSx());
16902         assertNull(calibrator.getEstimatedSy());
16903         assertNull(calibrator.getEstimatedSz());
16904         assertNull(calibrator.getEstimatedMxy());
16905         assertNull(calibrator.getEstimatedMxz());
16906         assertNull(calibrator.getEstimatedMyx());
16907         assertNull(calibrator.getEstimatedMyz());
16908         assertNull(calibrator.getEstimatedMzx());
16909         assertNull(calibrator.getEstimatedMzy());
16910         assertNull(calibrator.getEstimatedCovariance());
16911         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16912         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16913                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16914         assertNotNull(calibrator.getGroundTruthGravityNorm());
16915         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16916         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16917         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16918                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16919         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16920         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16921         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16922 
16923         // Force IllegalArgumentException
16924         calibrator = null;
16925         try {
16926             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16927                     measurements, new double[1], this);
16928             fail("IllegalArgumentException expected but not thrown");
16929         } catch (final IllegalArgumentException ignore) {
16930         }
16931         assertNull(calibrator);
16932     }
16933 
16934     @Test
16935     public void testConstructor137() throws WrongSizeException {
16936         final Matrix ba = generateBa();
16937         final double[] bias = ba.getBuffer();
16938         final double biasX = ba.getElementAtIndex(0);
16939         final double biasY = ba.getElementAtIndex(1);
16940         final double biasZ = ba.getElementAtIndex(2);
16941 
16942         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16943         final double latitude = Math.toRadians(
16944                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16945         final double longitude = Math.toRadians(
16946                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16947         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16948         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16949         final NEDVelocity nedVelocity = new NEDVelocity();
16950         final ECEFPosition ecefPosition = new ECEFPosition();
16951         final ECEFVelocity ecefVelocity = new ECEFVelocity();
16952         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16953                 ecefPosition, ecefVelocity);
16954 
16955         KnownPositionAccelerometerCalibrator calibrator =
16956                 new KnownPositionAccelerometerCalibrator(ecefPosition,
16957                         true, bias);
16958 
16959         // check default values
16960         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16961         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16962         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16963         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16964         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16965         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16966         final Acceleration bx2 = new Acceleration(0.0,
16967                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16968         calibrator.getInitialBiasXAsAcceleration(bx2);
16969         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16970         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16971         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16972         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16973         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16974         final Acceleration by2 = new Acceleration(0.0,
16975                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16976         calibrator.getInitialBiasYAsAcceleration(by2);
16977         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16978         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16979         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16980         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16981         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16982         final Acceleration bz2 = new Acceleration(0.0,
16983                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16984         calibrator.getInitialBiasZAsAcceleration(bz2);
16985         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16986         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16987         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16988         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16989         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16990         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16991         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16992         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16993         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16994         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16995         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16996         final double[] bias1 = calibrator.getInitialBias();
16997         assertArrayEquals(bias1, bias, 0.0);
16998         final double[] bias2 = new double[3];
16999         calibrator.getInitialBias(bias2);
17000         assertArrayEquals(bias1, bias2, 0.0);
17001         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17002         assertEquals(b1, ba);
17003         final Matrix b2 = new Matrix(3, 1);
17004         calibrator.getInitialBiasAsMatrix(b2);
17005         assertEquals(b1, b2);
17006         final Matrix ma1 = calibrator.getInitialMa();
17007         assertEquals(ma1, new Matrix(3, 3));
17008         final Matrix ma2 = new Matrix(3, 3);
17009         calibrator.getInitialMa(ma2);
17010         assertEquals(ma1, ma2);
17011         assertSame(calibrator.getEcefPosition(), ecefPosition);
17012         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17013         final NEDPosition nedPosition1 = new NEDPosition();
17014         assertTrue(calibrator.getNedPosition(nedPosition1));
17015         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17016         assertNull(calibrator.getMeasurements());
17017         assertTrue(calibrator.isCommonAxisUsed());
17018         assertNull(calibrator.getListener());
17019         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17020         assertFalse(calibrator.isReady());
17021         assertFalse(calibrator.isRunning());
17022         assertNull(calibrator.getEstimatedBiases());
17023         assertFalse(calibrator.getEstimatedBiases(null));
17024         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17025         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17026         assertNull(calibrator.getEstimatedBiasFx());
17027         assertNull(calibrator.getEstimatedBiasFy());
17028         assertNull(calibrator.getEstimatedBiasFz());
17029         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17030         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17031         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17032         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17033         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17034         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17035         assertNull(calibrator.getEstimatedMa());
17036         assertNull(calibrator.getEstimatedSx());
17037         assertNull(calibrator.getEstimatedSy());
17038         assertNull(calibrator.getEstimatedSz());
17039         assertNull(calibrator.getEstimatedMxy());
17040         assertNull(calibrator.getEstimatedMxz());
17041         assertNull(calibrator.getEstimatedMyx());
17042         assertNull(calibrator.getEstimatedMyz());
17043         assertNull(calibrator.getEstimatedMzx());
17044         assertNull(calibrator.getEstimatedMzy());
17045         assertNull(calibrator.getEstimatedCovariance());
17046         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17047         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17048                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17049         assertNotNull(calibrator.getGroundTruthGravityNorm());
17050         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17051         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17052         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17053                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17054         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17055         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17056         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17057 
17058         // Force IllegalArgumentException
17059         calibrator = null;
17060         try {
17061             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17062                     true, new double[1]);
17063             fail("IllegalArgumentException expected but not thrown");
17064         } catch (final IllegalArgumentException ignore) {
17065         }
17066         assertNull(calibrator);
17067     }
17068 
17069     @Test
17070     public void testConstructor138() throws WrongSizeException {
17071         final Matrix ba = generateBa();
17072         final double[] bias = ba.getBuffer();
17073         final double biasX = ba.getElementAtIndex(0);
17074         final double biasY = ba.getElementAtIndex(1);
17075         final double biasZ = ba.getElementAtIndex(2);
17076 
17077         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17078         final double latitude = Math.toRadians(
17079                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17080         final double longitude = Math.toRadians(
17081                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17082         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17083         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17084         final NEDVelocity nedVelocity = new NEDVelocity();
17085         final ECEFPosition ecefPosition = new ECEFPosition();
17086         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17087         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17088                 ecefPosition, ecefVelocity);
17089 
17090         KnownPositionAccelerometerCalibrator calibrator =
17091                 new KnownPositionAccelerometerCalibrator(ecefPosition,
17092                         true, bias, this);
17093 
17094         // check default values
17095         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17096         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17097         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17098         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17099         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17100         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17101         final Acceleration bx2 = new Acceleration(0.0,
17102                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17103         calibrator.getInitialBiasXAsAcceleration(bx2);
17104         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17105         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17106         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17107         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17108         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17109         final Acceleration by2 = new Acceleration(0.0,
17110                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17111         calibrator.getInitialBiasYAsAcceleration(by2);
17112         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17113         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17114         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17115         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17116         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17117         final Acceleration bz2 = new Acceleration(0.0,
17118                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17119         calibrator.getInitialBiasZAsAcceleration(bz2);
17120         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17121         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17122         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17123         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17124         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17125         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17126         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17127         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17128         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17129         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17130         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17131         final double[] bias1 = calibrator.getInitialBias();
17132         assertArrayEquals(bias1, bias, 0.0);
17133         final double[] bias2 = new double[3];
17134         calibrator.getInitialBias(bias2);
17135         assertArrayEquals(bias1, bias2, 0.0);
17136         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17137         assertEquals(b1, ba);
17138         final Matrix b2 = new Matrix(3, 1);
17139         calibrator.getInitialBiasAsMatrix(b2);
17140         assertEquals(b1, b2);
17141         final Matrix ma1 = calibrator.getInitialMa();
17142         assertEquals(ma1, new Matrix(3, 3));
17143         final Matrix ma2 = new Matrix(3, 3);
17144         calibrator.getInitialMa(ma2);
17145         assertEquals(ma1, ma2);
17146         assertSame(calibrator.getEcefPosition(), ecefPosition);
17147         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17148         final NEDPosition nedPosition1 = new NEDPosition();
17149         assertTrue(calibrator.getNedPosition(nedPosition1));
17150         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17151         assertNull(calibrator.getMeasurements());
17152         assertTrue(calibrator.isCommonAxisUsed());
17153         assertSame(calibrator.getListener(), this);
17154         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17155         assertFalse(calibrator.isReady());
17156         assertFalse(calibrator.isRunning());
17157         assertNull(calibrator.getEstimatedBiases());
17158         assertFalse(calibrator.getEstimatedBiases(null));
17159         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17160         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17161         assertNull(calibrator.getEstimatedBiasFx());
17162         assertNull(calibrator.getEstimatedBiasFy());
17163         assertNull(calibrator.getEstimatedBiasFz());
17164         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17165         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17166         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17167         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17168         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17169         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17170         assertNull(calibrator.getEstimatedMa());
17171         assertNull(calibrator.getEstimatedSx());
17172         assertNull(calibrator.getEstimatedSy());
17173         assertNull(calibrator.getEstimatedSz());
17174         assertNull(calibrator.getEstimatedMxy());
17175         assertNull(calibrator.getEstimatedMxz());
17176         assertNull(calibrator.getEstimatedMyx());
17177         assertNull(calibrator.getEstimatedMyz());
17178         assertNull(calibrator.getEstimatedMzx());
17179         assertNull(calibrator.getEstimatedMzy());
17180         assertNull(calibrator.getEstimatedCovariance());
17181         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17182         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17183                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17184         assertNotNull(calibrator.getGroundTruthGravityNorm());
17185         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17186         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17187         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17188                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17189         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17190         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17191         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17192 
17193         // Force IllegalArgumentException
17194         calibrator = null;
17195         try {
17196             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17197                     true, new double[1], this);
17198             fail("IllegalArgumentException expected but not thrown");
17199         } catch (final IllegalArgumentException ignore) {
17200         }
17201         assertNull(calibrator);
17202     }
17203 
17204     @Test
17205     public void testConstructor139() throws WrongSizeException {
17206         final Collection<StandardDeviationBodyKinematics> measurements =
17207                 Collections.emptyList();
17208 
17209         final Matrix ba = generateBa();
17210         final double[] bias = ba.getBuffer();
17211         final double biasX = ba.getElementAtIndex(0);
17212         final double biasY = ba.getElementAtIndex(1);
17213         final double biasZ = ba.getElementAtIndex(2);
17214 
17215         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17216         final double latitude = Math.toRadians(
17217                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17218         final double longitude = Math.toRadians(
17219                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17220         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17221         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17222         final NEDVelocity nedVelocity = new NEDVelocity();
17223         final ECEFPosition ecefPosition = new ECEFPosition();
17224         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17225         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17226                 ecefPosition, ecefVelocity);
17227 
17228         KnownPositionAccelerometerCalibrator calibrator =
17229                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17230                         true, bias);
17231 
17232         // check default values
17233         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17234         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17235         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17236         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17237         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17238         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17239         final Acceleration bx2 = new Acceleration(0.0,
17240                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17241         calibrator.getInitialBiasXAsAcceleration(bx2);
17242         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17243         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17244         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17245         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17246         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17247         final Acceleration by2 = new Acceleration(0.0,
17248                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17249         calibrator.getInitialBiasYAsAcceleration(by2);
17250         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17251         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17252         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17253         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17254         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17255         final Acceleration bz2 = new Acceleration(0.0,
17256                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17257         calibrator.getInitialBiasZAsAcceleration(bz2);
17258         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17259         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17260         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17261         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17262         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17263         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17264         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17265         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17266         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17267         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17268         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17269         final double[] bias1 = calibrator.getInitialBias();
17270         assertArrayEquals(bias1, bias, 0.0);
17271         final double[] bias2 = new double[3];
17272         calibrator.getInitialBias(bias2);
17273         assertArrayEquals(bias1, bias2, 0.0);
17274         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17275         assertEquals(b1, ba);
17276         final Matrix b2 = new Matrix(3, 1);
17277         calibrator.getInitialBiasAsMatrix(b2);
17278         assertEquals(b1, b2);
17279         final Matrix ma1 = calibrator.getInitialMa();
17280         assertEquals(ma1, new Matrix(3, 3));
17281         final Matrix ma2 = new Matrix(3, 3);
17282         calibrator.getInitialMa(ma2);
17283         assertEquals(ma1, ma2);
17284         assertSame(calibrator.getEcefPosition(), ecefPosition);
17285         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17286         final NEDPosition nedPosition1 = new NEDPosition();
17287         assertTrue(calibrator.getNedPosition(nedPosition1));
17288         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17289         assertSame(calibrator.getMeasurements(), measurements);
17290         assertTrue(calibrator.isCommonAxisUsed());
17291         assertNull(calibrator.getListener());
17292         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17293         assertFalse(calibrator.isReady());
17294         assertFalse(calibrator.isRunning());
17295         assertNull(calibrator.getEstimatedBiases());
17296         assertFalse(calibrator.getEstimatedBiases(null));
17297         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17298         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17299         assertNull(calibrator.getEstimatedBiasFx());
17300         assertNull(calibrator.getEstimatedBiasFy());
17301         assertNull(calibrator.getEstimatedBiasFz());
17302         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17303         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17304         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17305         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17306         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17307         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17308         assertNull(calibrator.getEstimatedMa());
17309         assertNull(calibrator.getEstimatedSx());
17310         assertNull(calibrator.getEstimatedSy());
17311         assertNull(calibrator.getEstimatedSz());
17312         assertNull(calibrator.getEstimatedMxy());
17313         assertNull(calibrator.getEstimatedMxz());
17314         assertNull(calibrator.getEstimatedMyx());
17315         assertNull(calibrator.getEstimatedMyz());
17316         assertNull(calibrator.getEstimatedMzx());
17317         assertNull(calibrator.getEstimatedMzy());
17318         assertNull(calibrator.getEstimatedCovariance());
17319         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17320         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17321                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17322         assertNotNull(calibrator.getGroundTruthGravityNorm());
17323         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17324         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17325         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17326                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17327         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17328         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17329         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17330 
17331         // Force IllegalArgumentException
17332         calibrator = null;
17333         try {
17334             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17335                     measurements, true, new double[1]);
17336             fail("IllegalArgumentException expected but not thrown");
17337         } catch (final IllegalArgumentException ignore) {
17338         }
17339         assertNull(calibrator);
17340     }
17341 
17342     @Test
17343     public void testConstructor140() throws WrongSizeException {
17344         final Collection<StandardDeviationBodyKinematics> measurements =
17345                 Collections.emptyList();
17346 
17347         final Matrix ba = generateBa();
17348         final double[] bias = ba.getBuffer();
17349         final double biasX = ba.getElementAtIndex(0);
17350         final double biasY = ba.getElementAtIndex(1);
17351         final double biasZ = ba.getElementAtIndex(2);
17352 
17353         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17354         final double latitude = Math.toRadians(
17355                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17356         final double longitude = Math.toRadians(
17357                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17358         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17359         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17360         final NEDVelocity nedVelocity = new NEDVelocity();
17361         final ECEFPosition ecefPosition = new ECEFPosition();
17362         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17363         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17364                 ecefPosition, ecefVelocity);
17365 
17366         KnownPositionAccelerometerCalibrator calibrator =
17367                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17368                         true, bias, this);
17369 
17370         // check default values
17371         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17372         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17373         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17374         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17375         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17376         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17377         final Acceleration bx2 = new Acceleration(0.0,
17378                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17379         calibrator.getInitialBiasXAsAcceleration(bx2);
17380         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17381         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17382         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17383         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17384         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17385         final Acceleration by2 = new Acceleration(0.0,
17386                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17387         calibrator.getInitialBiasYAsAcceleration(by2);
17388         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17389         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17390         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17391         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17392         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17393         final Acceleration bz2 = new Acceleration(0.0,
17394                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17395         calibrator.getInitialBiasZAsAcceleration(bz2);
17396         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17397         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17398         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17399         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17400         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17401         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17402         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17403         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17404         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17405         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17406         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17407         final double[] bias1 = calibrator.getInitialBias();
17408         assertArrayEquals(bias1, bias, 0.0);
17409         final double[] bias2 = new double[3];
17410         calibrator.getInitialBias(bias2);
17411         assertArrayEquals(bias1, bias2, 0.0);
17412         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17413         assertEquals(b1, ba);
17414         final Matrix b2 = new Matrix(3, 1);
17415         calibrator.getInitialBiasAsMatrix(b2);
17416         assertEquals(b1, b2);
17417         final Matrix ma1 = calibrator.getInitialMa();
17418         assertEquals(ma1, new Matrix(3, 3));
17419         final Matrix ma2 = new Matrix(3, 3);
17420         calibrator.getInitialMa(ma2);
17421         assertEquals(ma1, ma2);
17422         assertSame(calibrator.getEcefPosition(), ecefPosition);
17423         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17424         final NEDPosition nedPosition1 = new NEDPosition();
17425         assertTrue(calibrator.getNedPosition(nedPosition1));
17426         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17427         assertSame(calibrator.getMeasurements(), measurements);
17428         assertTrue(calibrator.isCommonAxisUsed());
17429         assertSame(calibrator.getListener(), this);
17430         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17431         assertFalse(calibrator.isReady());
17432         assertFalse(calibrator.isRunning());
17433         assertNull(calibrator.getEstimatedBiases());
17434         assertFalse(calibrator.getEstimatedBiases(null));
17435         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17436         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17437         assertNull(calibrator.getEstimatedBiasFx());
17438         assertNull(calibrator.getEstimatedBiasFy());
17439         assertNull(calibrator.getEstimatedBiasFz());
17440         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17441         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17442         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17443         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17444         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17445         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17446         assertNull(calibrator.getEstimatedMa());
17447         assertNull(calibrator.getEstimatedSx());
17448         assertNull(calibrator.getEstimatedSy());
17449         assertNull(calibrator.getEstimatedSz());
17450         assertNull(calibrator.getEstimatedMxy());
17451         assertNull(calibrator.getEstimatedMxz());
17452         assertNull(calibrator.getEstimatedMyx());
17453         assertNull(calibrator.getEstimatedMyz());
17454         assertNull(calibrator.getEstimatedMzx());
17455         assertNull(calibrator.getEstimatedMzy());
17456         assertNull(calibrator.getEstimatedCovariance());
17457         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17458         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17459                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17460         assertNotNull(calibrator.getGroundTruthGravityNorm());
17461         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17462         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17463         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17464                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17465         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17466         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17467         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17468 
17469         // Force IllegalArgumentException
17470         calibrator = null;
17471         try {
17472             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17473                     measurements, true, new double[1], this);
17474             fail("IllegalArgumentException expected but not thrown");
17475         } catch (final IllegalArgumentException ignore) {
17476         }
17477         assertNull(calibrator);
17478     }
17479 
17480     @Test
17481     public void testConstructor141() throws WrongSizeException {
17482         final Matrix ba = generateBa();
17483         final double[] bias = ba.getBuffer();
17484         final double biasX = ba.getElementAtIndex(0);
17485         final double biasY = ba.getElementAtIndex(1);
17486         final double biasZ = ba.getElementAtIndex(2);
17487 
17488         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17489         final double latitude = Math.toRadians(
17490                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17491         final double longitude = Math.toRadians(
17492                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17493         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17494         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17495         final NEDVelocity nedVelocity = new NEDVelocity();
17496         final ECEFPosition ecefPosition = new ECEFPosition();
17497         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17498         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17499                 ecefPosition, ecefVelocity);
17500 
17501         KnownPositionAccelerometerCalibrator calibrator =
17502                 new KnownPositionAccelerometerCalibrator(ecefPosition, ba);
17503 
17504         // check default values
17505         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17506         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17507         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17508         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17509         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17510         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17511         final Acceleration bx2 = new Acceleration(0.0,
17512                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17513         calibrator.getInitialBiasXAsAcceleration(bx2);
17514         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17515         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17516         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17517         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17518         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17519         final Acceleration by2 = new Acceleration(0.0,
17520                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17521         calibrator.getInitialBiasYAsAcceleration(by2);
17522         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17523         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17524         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17525         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17526         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17527         final Acceleration bz2 = new Acceleration(0.0,
17528                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17529         calibrator.getInitialBiasZAsAcceleration(bz2);
17530         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17531         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17532         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17533         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17534         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17535         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17536         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17537         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17538         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17539         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17540         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17541         final double[] bias1 = calibrator.getInitialBias();
17542         assertArrayEquals(bias1, bias, 0.0);
17543         final double[] bias2 = new double[3];
17544         calibrator.getInitialBias(bias2);
17545         assertArrayEquals(bias1, bias2, 0.0);
17546         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17547         assertEquals(b1, ba);
17548         final Matrix b2 = new Matrix(3, 1);
17549         calibrator.getInitialBiasAsMatrix(b2);
17550         assertEquals(b1, b2);
17551         final Matrix ma1 = calibrator.getInitialMa();
17552         assertEquals(ma1, new Matrix(3, 3));
17553         final Matrix ma2 = new Matrix(3, 3);
17554         calibrator.getInitialMa(ma2);
17555         assertEquals(ma1, ma2);
17556         assertSame(calibrator.getEcefPosition(), ecefPosition);
17557         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17558         final NEDPosition nedPosition1 = new NEDPosition();
17559         assertTrue(calibrator.getNedPosition(nedPosition1));
17560         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17561         assertNull(calibrator.getMeasurements());
17562         assertFalse(calibrator.isCommonAxisUsed());
17563         assertNull(calibrator.getListener());
17564         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17565         assertFalse(calibrator.isReady());
17566         assertFalse(calibrator.isRunning());
17567         assertNull(calibrator.getEstimatedBiases());
17568         assertFalse(calibrator.getEstimatedBiases(null));
17569         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17570         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17571         assertNull(calibrator.getEstimatedBiasFx());
17572         assertNull(calibrator.getEstimatedBiasFy());
17573         assertNull(calibrator.getEstimatedBiasFz());
17574         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17575         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17576         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17577         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17578         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17579         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17580         assertNull(calibrator.getEstimatedMa());
17581         assertNull(calibrator.getEstimatedSx());
17582         assertNull(calibrator.getEstimatedSy());
17583         assertNull(calibrator.getEstimatedSz());
17584         assertNull(calibrator.getEstimatedMxy());
17585         assertNull(calibrator.getEstimatedMxz());
17586         assertNull(calibrator.getEstimatedMyx());
17587         assertNull(calibrator.getEstimatedMyz());
17588         assertNull(calibrator.getEstimatedMzx());
17589         assertNull(calibrator.getEstimatedMzy());
17590         assertNull(calibrator.getEstimatedCovariance());
17591         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17592         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17593                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17594         assertNotNull(calibrator.getGroundTruthGravityNorm());
17595         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17596         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17597         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17598                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17599         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17600         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17601         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17602 
17603         // Force IllegalArgumentException
17604         calibrator = null;
17605         try {
17606             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17607                     new Matrix(1, 1));
17608             fail("IllegalArgumentException expected but not thrown");
17609         } catch (final IllegalArgumentException ignore) {
17610         }
17611         try {
17612             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17613                     new Matrix(1, 3));
17614             fail("IllegalArgumentException expected but not thrown");
17615         } catch (final IllegalArgumentException ignore) {
17616         }
17617         assertNull(calibrator);
17618     }
17619 
17620     @Test
17621     public void testConstructor142() throws WrongSizeException {
17622         final Matrix ba = generateBa();
17623         final double[] bias = ba.getBuffer();
17624         final double biasX = ba.getElementAtIndex(0);
17625         final double biasY = ba.getElementAtIndex(1);
17626         final double biasZ = ba.getElementAtIndex(2);
17627 
17628         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17629         final double latitude = Math.toRadians(
17630                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17631         final double longitude = Math.toRadians(
17632                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17633         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17634         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17635         final NEDVelocity nedVelocity = new NEDVelocity();
17636         final ECEFPosition ecefPosition = new ECEFPosition();
17637         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17638         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17639                 ecefPosition, ecefVelocity);
17640 
17641         KnownPositionAccelerometerCalibrator calibrator =
17642                 new KnownPositionAccelerometerCalibrator(ecefPosition, ba,
17643                         this);
17644 
17645         // check default values
17646         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17647         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17648         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17649         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17650         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17651         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17652         final Acceleration bx2 = new Acceleration(0.0,
17653                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17654         calibrator.getInitialBiasXAsAcceleration(bx2);
17655         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17656         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17657         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17658         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17659         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17660         final Acceleration by2 = new Acceleration(0.0,
17661                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17662         calibrator.getInitialBiasYAsAcceleration(by2);
17663         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17664         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17665         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17666         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17667         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17668         final Acceleration bz2 = new Acceleration(0.0,
17669                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17670         calibrator.getInitialBiasZAsAcceleration(bz2);
17671         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17672         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17673         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17674         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17675         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17676         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17677         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17678         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17679         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17680         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17681         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17682         final double[] bias1 = calibrator.getInitialBias();
17683         assertArrayEquals(bias1, bias, 0.0);
17684         final double[] bias2 = new double[3];
17685         calibrator.getInitialBias(bias2);
17686         assertArrayEquals(bias1, bias2, 0.0);
17687         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17688         assertEquals(b1, ba);
17689         final Matrix b2 = new Matrix(3, 1);
17690         calibrator.getInitialBiasAsMatrix(b2);
17691         assertEquals(b1, b2);
17692         final Matrix ma1 = calibrator.getInitialMa();
17693         assertEquals(ma1, new Matrix(3, 3));
17694         final Matrix ma2 = new Matrix(3, 3);
17695         calibrator.getInitialMa(ma2);
17696         assertEquals(ma1, ma2);
17697         assertSame(calibrator.getEcefPosition(), ecefPosition);
17698         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17699         final NEDPosition nedPosition1 = new NEDPosition();
17700         assertTrue(calibrator.getNedPosition(nedPosition1));
17701         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17702         assertNull(calibrator.getMeasurements());
17703         assertFalse(calibrator.isCommonAxisUsed());
17704         assertSame(calibrator.getListener(), this);
17705         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17706         assertFalse(calibrator.isReady());
17707         assertFalse(calibrator.isRunning());
17708         assertNull(calibrator.getEstimatedBiases());
17709         assertFalse(calibrator.getEstimatedBiases(null));
17710         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17711         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17712         assertNull(calibrator.getEstimatedBiasFx());
17713         assertNull(calibrator.getEstimatedBiasFy());
17714         assertNull(calibrator.getEstimatedBiasFz());
17715         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17716         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17717         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17718         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17719         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17720         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17721         assertNull(calibrator.getEstimatedMa());
17722         assertNull(calibrator.getEstimatedSx());
17723         assertNull(calibrator.getEstimatedSy());
17724         assertNull(calibrator.getEstimatedSz());
17725         assertNull(calibrator.getEstimatedMxy());
17726         assertNull(calibrator.getEstimatedMxz());
17727         assertNull(calibrator.getEstimatedMyx());
17728         assertNull(calibrator.getEstimatedMyz());
17729         assertNull(calibrator.getEstimatedMzx());
17730         assertNull(calibrator.getEstimatedMzy());
17731         assertNull(calibrator.getEstimatedCovariance());
17732         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17733         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17734                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17735         assertNotNull(calibrator.getGroundTruthGravityNorm());
17736         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17737         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17738         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17739                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17740         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17741         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17742         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17743 
17744         // Force IllegalArgumentException
17745         calibrator = null;
17746         try {
17747             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17748                     new Matrix(1, 1), this);
17749             fail("IllegalArgumentException expected but not thrown");
17750         } catch (final IllegalArgumentException ignore) {
17751         }
17752         try {
17753             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17754                     new Matrix(1, 3), this);
17755             fail("IllegalArgumentException expected but not thrown");
17756         } catch (final IllegalArgumentException ignore) {
17757         }
17758         assertNull(calibrator);
17759     }
17760 
17761     @Test
17762     public void testConstructor143() throws WrongSizeException {
17763         final Collection<StandardDeviationBodyKinematics> measurements =
17764                 Collections.emptyList();
17765 
17766         final Matrix ba = generateBa();
17767         final double[] bias = ba.getBuffer();
17768         final double biasX = ba.getElementAtIndex(0);
17769         final double biasY = ba.getElementAtIndex(1);
17770         final double biasZ = ba.getElementAtIndex(2);
17771 
17772         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17773         final double latitude = Math.toRadians(
17774                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17775         final double longitude = Math.toRadians(
17776                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17777         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17778         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17779         final NEDVelocity nedVelocity = new NEDVelocity();
17780         final ECEFPosition ecefPosition = new ECEFPosition();
17781         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17782         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17783                 ecefPosition, ecefVelocity);
17784 
17785         KnownPositionAccelerometerCalibrator calibrator =
17786                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17787                         ba);
17788 
17789         // check default values
17790         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17791         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17792         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17793         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17794         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17795         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17796         final Acceleration bx2 = new Acceleration(0.0,
17797                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17798         calibrator.getInitialBiasXAsAcceleration(bx2);
17799         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17800         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17801         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17802         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17803         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17804         final Acceleration by2 = new Acceleration(0.0,
17805                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17806         calibrator.getInitialBiasYAsAcceleration(by2);
17807         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17808         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17809         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17810         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17811         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17812         final Acceleration bz2 = new Acceleration(0.0,
17813                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17814         calibrator.getInitialBiasZAsAcceleration(bz2);
17815         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17816         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17817         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17818         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17819         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17820         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17821         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17822         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17823         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17824         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17825         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17826         final double[] bias1 = calibrator.getInitialBias();
17827         assertArrayEquals(bias1, bias, 0.0);
17828         final double[] bias2 = new double[3];
17829         calibrator.getInitialBias(bias2);
17830         assertArrayEquals(bias1, bias2, 0.0);
17831         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17832         assertEquals(b1, ba);
17833         final Matrix b2 = new Matrix(3, 1);
17834         calibrator.getInitialBiasAsMatrix(b2);
17835         assertEquals(b1, b2);
17836         final Matrix ma1 = calibrator.getInitialMa();
17837         assertEquals(ma1, new Matrix(3, 3));
17838         final Matrix ma2 = new Matrix(3, 3);
17839         calibrator.getInitialMa(ma2);
17840         assertEquals(ma1, ma2);
17841         assertSame(calibrator.getEcefPosition(), ecefPosition);
17842         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17843         final NEDPosition nedPosition1 = new NEDPosition();
17844         assertTrue(calibrator.getNedPosition(nedPosition1));
17845         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17846         assertSame(calibrator.getMeasurements(), measurements);
17847         assertFalse(calibrator.isCommonAxisUsed());
17848         assertNull(calibrator.getListener());
17849         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17850         assertFalse(calibrator.isReady());
17851         assertFalse(calibrator.isRunning());
17852         assertNull(calibrator.getEstimatedBiases());
17853         assertFalse(calibrator.getEstimatedBiases(null));
17854         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17855         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17856         assertNull(calibrator.getEstimatedBiasFx());
17857         assertNull(calibrator.getEstimatedBiasFy());
17858         assertNull(calibrator.getEstimatedBiasFz());
17859         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17860         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17861         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17862         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17863         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17864         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17865         assertNull(calibrator.getEstimatedMa());
17866         assertNull(calibrator.getEstimatedSx());
17867         assertNull(calibrator.getEstimatedSy());
17868         assertNull(calibrator.getEstimatedSz());
17869         assertNull(calibrator.getEstimatedMxy());
17870         assertNull(calibrator.getEstimatedMxz());
17871         assertNull(calibrator.getEstimatedMyx());
17872         assertNull(calibrator.getEstimatedMyz());
17873         assertNull(calibrator.getEstimatedMzx());
17874         assertNull(calibrator.getEstimatedMzy());
17875         assertNull(calibrator.getEstimatedCovariance());
17876         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17877         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17878                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17879         assertNotNull(calibrator.getGroundTruthGravityNorm());
17880         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17881         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17882         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17883                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17884         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17885         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17886         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17887 
17888         // Force IllegalArgumentException
17889         calibrator = null;
17890         try {
17891             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17892                     measurements, new Matrix(1, 1));
17893             fail("IllegalArgumentException expected but not thrown");
17894         } catch (final IllegalArgumentException ignore) {
17895         }
17896         try {
17897             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17898                     measurements, new Matrix(1, 3));
17899             fail("IllegalArgumentException expected but not thrown");
17900         } catch (final IllegalArgumentException ignore) {
17901         }
17902         assertNull(calibrator);
17903     }
17904 
17905     @Test
17906     public void testConstructor144() throws WrongSizeException {
17907         final Collection<StandardDeviationBodyKinematics> measurements =
17908                 Collections.emptyList();
17909 
17910         final Matrix ba = generateBa();
17911         final double[] bias = ba.getBuffer();
17912         final double biasX = ba.getElementAtIndex(0);
17913         final double biasY = ba.getElementAtIndex(1);
17914         final double biasZ = ba.getElementAtIndex(2);
17915 
17916         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17917         final double latitude = Math.toRadians(
17918                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17919         final double longitude = Math.toRadians(
17920                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17921         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17922         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17923         final NEDVelocity nedVelocity = new NEDVelocity();
17924         final ECEFPosition ecefPosition = new ECEFPosition();
17925         final ECEFVelocity ecefVelocity = new ECEFVelocity();
17926         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17927                 ecefPosition, ecefVelocity);
17928 
17929         KnownPositionAccelerometerCalibrator calibrator =
17930                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17931                         ba, this);
17932 
17933         // check default values
17934         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17935         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17936         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17937         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17938         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17939         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17940         final Acceleration bx2 = new Acceleration(0.0,
17941                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17942         calibrator.getInitialBiasXAsAcceleration(bx2);
17943         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17944         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17945         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17946         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17947         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17948         final Acceleration by2 = new Acceleration(0.0,
17949                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17950         calibrator.getInitialBiasYAsAcceleration(by2);
17951         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17952         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17953         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17954         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17955         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17956         final Acceleration bz2 = new Acceleration(0.0,
17957                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17958         calibrator.getInitialBiasZAsAcceleration(bz2);
17959         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17960         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17961         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17962         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17963         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17964         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17965         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17966         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17967         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17968         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17969         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17970         final double[] bias1 = calibrator.getInitialBias();
17971         assertArrayEquals(bias1, bias, 0.0);
17972         final double[] bias2 = new double[3];
17973         calibrator.getInitialBias(bias2);
17974         assertArrayEquals(bias1, bias2, 0.0);
17975         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17976         assertEquals(b1, ba);
17977         final Matrix b2 = new Matrix(3, 1);
17978         calibrator.getInitialBiasAsMatrix(b2);
17979         assertEquals(b1, b2);
17980         final Matrix ma1 = calibrator.getInitialMa();
17981         assertEquals(ma1, new Matrix(3, 3));
17982         final Matrix ma2 = new Matrix(3, 3);
17983         calibrator.getInitialMa(ma2);
17984         assertEquals(ma1, ma2);
17985         assertSame(calibrator.getEcefPosition(), ecefPosition);
17986         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17987         final NEDPosition nedPosition1 = new NEDPosition();
17988         assertTrue(calibrator.getNedPosition(nedPosition1));
17989         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17990         assertSame(calibrator.getMeasurements(), measurements);
17991         assertFalse(calibrator.isCommonAxisUsed());
17992         assertSame(calibrator.getListener(), this);
17993         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17994         assertFalse(calibrator.isReady());
17995         assertFalse(calibrator.isRunning());
17996         assertNull(calibrator.getEstimatedBiases());
17997         assertFalse(calibrator.getEstimatedBiases(null));
17998         assertNull(calibrator.getEstimatedBiasesAsMatrix());
17999         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18000         assertNull(calibrator.getEstimatedBiasFx());
18001         assertNull(calibrator.getEstimatedBiasFy());
18002         assertNull(calibrator.getEstimatedBiasFz());
18003         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18004         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18005         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18006         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18007         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18008         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18009         assertNull(calibrator.getEstimatedMa());
18010         assertNull(calibrator.getEstimatedSx());
18011         assertNull(calibrator.getEstimatedSy());
18012         assertNull(calibrator.getEstimatedSz());
18013         assertNull(calibrator.getEstimatedMxy());
18014         assertNull(calibrator.getEstimatedMxz());
18015         assertNull(calibrator.getEstimatedMyx());
18016         assertNull(calibrator.getEstimatedMyz());
18017         assertNull(calibrator.getEstimatedMzx());
18018         assertNull(calibrator.getEstimatedMzy());
18019         assertNull(calibrator.getEstimatedCovariance());
18020         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18021         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18022                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18023         assertNotNull(calibrator.getGroundTruthGravityNorm());
18024         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18025         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18026         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18027                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18028         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18029         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18030         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18031 
18032         // Force IllegalArgumentException
18033         calibrator = null;
18034         try {
18035             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18036                     measurements, new Matrix(1, 1), this);
18037             fail("IllegalArgumentException expected but not thrown");
18038         } catch (final IllegalArgumentException ignore) {
18039         }
18040         try {
18041             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18042                     measurements, new Matrix(1, 3), this);
18043             fail("IllegalArgumentException expected but not thrown");
18044         } catch (final IllegalArgumentException ignore) {
18045         }
18046         assertNull(calibrator);
18047     }
18048 
18049     @Test
18050     public void testConstructor145() throws WrongSizeException {
18051         final Matrix ba = generateBa();
18052         final double[] bias = ba.getBuffer();
18053         final double biasX = ba.getElementAtIndex(0);
18054         final double biasY = ba.getElementAtIndex(1);
18055         final double biasZ = ba.getElementAtIndex(2);
18056 
18057         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18058         final double latitude = Math.toRadians(
18059                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18060         final double longitude = Math.toRadians(
18061                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18062         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18063         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18064         final NEDVelocity nedVelocity = new NEDVelocity();
18065         final ECEFPosition ecefPosition = new ECEFPosition();
18066         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18067         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18068                 ecefPosition, ecefVelocity);
18069 
18070         KnownPositionAccelerometerCalibrator calibrator =
18071                 new KnownPositionAccelerometerCalibrator(ecefPosition,
18072                         true, ba);
18073 
18074         // check default values
18075         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18076         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18077         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18078         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18079         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18080         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18081         final Acceleration bx2 = new Acceleration(0.0,
18082                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18083         calibrator.getInitialBiasXAsAcceleration(bx2);
18084         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18085         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18086         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18087         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18088         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18089         final Acceleration by2 = new Acceleration(0.0,
18090                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18091         calibrator.getInitialBiasYAsAcceleration(by2);
18092         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18093         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18094         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18095         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18096         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18097         final Acceleration bz2 = new Acceleration(0.0,
18098                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18099         calibrator.getInitialBiasZAsAcceleration(bz2);
18100         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18101         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18102         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18103         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18104         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18105         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18106         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18107         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18108         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18109         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18110         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18111         final double[] bias1 = calibrator.getInitialBias();
18112         assertArrayEquals(bias1, bias, 0.0);
18113         final double[] bias2 = new double[3];
18114         calibrator.getInitialBias(bias2);
18115         assertArrayEquals(bias1, bias2, 0.0);
18116         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18117         assertEquals(b1, ba);
18118         final Matrix b2 = new Matrix(3, 1);
18119         calibrator.getInitialBiasAsMatrix(b2);
18120         assertEquals(b1, b2);
18121         final Matrix ma1 = calibrator.getInitialMa();
18122         assertEquals(ma1, new Matrix(3, 3));
18123         final Matrix ma2 = new Matrix(3, 3);
18124         calibrator.getInitialMa(ma2);
18125         assertEquals(ma1, ma2);
18126         assertSame(calibrator.getEcefPosition(), ecefPosition);
18127         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18128         final NEDPosition nedPosition1 = new NEDPosition();
18129         assertTrue(calibrator.getNedPosition(nedPosition1));
18130         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18131         assertNull(calibrator.getMeasurements());
18132         assertTrue(calibrator.isCommonAxisUsed());
18133         assertNull(calibrator.getListener());
18134         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18135         assertFalse(calibrator.isReady());
18136         assertFalse(calibrator.isRunning());
18137         assertNull(calibrator.getEstimatedBiases());
18138         assertFalse(calibrator.getEstimatedBiases(null));
18139         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18140         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18141         assertNull(calibrator.getEstimatedBiasFx());
18142         assertNull(calibrator.getEstimatedBiasFy());
18143         assertNull(calibrator.getEstimatedBiasFz());
18144         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18145         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18146         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18147         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18148         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18149         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18150         assertNull(calibrator.getEstimatedMa());
18151         assertNull(calibrator.getEstimatedSx());
18152         assertNull(calibrator.getEstimatedSy());
18153         assertNull(calibrator.getEstimatedSz());
18154         assertNull(calibrator.getEstimatedMxy());
18155         assertNull(calibrator.getEstimatedMxz());
18156         assertNull(calibrator.getEstimatedMyx());
18157         assertNull(calibrator.getEstimatedMyz());
18158         assertNull(calibrator.getEstimatedMzx());
18159         assertNull(calibrator.getEstimatedMzy());
18160         assertNull(calibrator.getEstimatedCovariance());
18161         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18162         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18163                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18164         assertNotNull(calibrator.getGroundTruthGravityNorm());
18165         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18166         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18167         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18168                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18169         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18170         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18171         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18172 
18173         // Force IllegalArgumentException
18174         calibrator = null;
18175         try {
18176             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18177                     true, new Matrix(1, 1));
18178             fail("IllegalArgumentException expected but not thrown");
18179         } catch (final IllegalArgumentException ignore) {
18180         }
18181         try {
18182             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18183                     true, new Matrix(1, 3));
18184             fail("IllegalArgumentException expected but not thrown");
18185         } catch (final IllegalArgumentException ignore) {
18186         }
18187         assertNull(calibrator);
18188     }
18189 
18190     @Test
18191     public void testConstructor146() throws WrongSizeException {
18192         final Matrix ba = generateBa();
18193         final double[] bias = ba.getBuffer();
18194         final double biasX = ba.getElementAtIndex(0);
18195         final double biasY = ba.getElementAtIndex(1);
18196         final double biasZ = ba.getElementAtIndex(2);
18197 
18198         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18199         final double latitude = Math.toRadians(
18200                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18201         final double longitude = Math.toRadians(
18202                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18203         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18204         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18205         final NEDVelocity nedVelocity = new NEDVelocity();
18206         final ECEFPosition ecefPosition = new ECEFPosition();
18207         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18208         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18209                 ecefPosition, ecefVelocity);
18210 
18211         KnownPositionAccelerometerCalibrator calibrator =
18212                 new KnownPositionAccelerometerCalibrator(ecefPosition,
18213                         true, ba, this);
18214 
18215         // check default values
18216         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18217         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18218         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18219         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18220         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18221         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18222         final Acceleration bx2 = new Acceleration(0.0,
18223                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18224         calibrator.getInitialBiasXAsAcceleration(bx2);
18225         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18226         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18227         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18228         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18229         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18230         final Acceleration by2 = new Acceleration(0.0,
18231                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18232         calibrator.getInitialBiasYAsAcceleration(by2);
18233         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18234         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18235         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18236         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18237         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18238         final Acceleration bz2 = new Acceleration(0.0,
18239                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18240         calibrator.getInitialBiasZAsAcceleration(bz2);
18241         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18242         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18243         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18244         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18245         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18246         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18247         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18248         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18249         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18250         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18251         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18252         final double[] bias1 = calibrator.getInitialBias();
18253         assertArrayEquals(bias1, bias, 0.0);
18254         final double[] bias2 = new double[3];
18255         calibrator.getInitialBias(bias2);
18256         assertArrayEquals(bias1, bias2, 0.0);
18257         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18258         assertEquals(b1, ba);
18259         final Matrix b2 = new Matrix(3, 1);
18260         calibrator.getInitialBiasAsMatrix(b2);
18261         assertEquals(b1, b2);
18262         final Matrix ma1 = calibrator.getInitialMa();
18263         assertEquals(ma1, new Matrix(3, 3));
18264         final Matrix ma2 = new Matrix(3, 3);
18265         calibrator.getInitialMa(ma2);
18266         assertEquals(ma1, ma2);
18267         assertSame(calibrator.getEcefPosition(), ecefPosition);
18268         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18269         final NEDPosition nedPosition1 = new NEDPosition();
18270         assertTrue(calibrator.getNedPosition(nedPosition1));
18271         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18272         assertNull(calibrator.getMeasurements());
18273         assertTrue(calibrator.isCommonAxisUsed());
18274         assertSame(calibrator.getListener(), this);
18275         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18276         assertFalse(calibrator.isReady());
18277         assertFalse(calibrator.isRunning());
18278         assertNull(calibrator.getEstimatedBiases());
18279         assertFalse(calibrator.getEstimatedBiases(null));
18280         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18281         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18282         assertNull(calibrator.getEstimatedBiasFx());
18283         assertNull(calibrator.getEstimatedBiasFy());
18284         assertNull(calibrator.getEstimatedBiasFz());
18285         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18286         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18287         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18288         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18289         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18290         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18291         assertNull(calibrator.getEstimatedMa());
18292         assertNull(calibrator.getEstimatedSx());
18293         assertNull(calibrator.getEstimatedSy());
18294         assertNull(calibrator.getEstimatedSz());
18295         assertNull(calibrator.getEstimatedMxy());
18296         assertNull(calibrator.getEstimatedMxz());
18297         assertNull(calibrator.getEstimatedMyx());
18298         assertNull(calibrator.getEstimatedMyz());
18299         assertNull(calibrator.getEstimatedMzx());
18300         assertNull(calibrator.getEstimatedMzy());
18301         assertNull(calibrator.getEstimatedCovariance());
18302         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18303         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18304                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18305         assertNotNull(calibrator.getGroundTruthGravityNorm());
18306         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18307         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18308         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18309                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18310         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18311         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18312         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18313 
18314         // Force IllegalArgumentException
18315         calibrator = null;
18316         try {
18317             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18318                     true, new Matrix(1, 1),
18319                     this);
18320             fail("IllegalArgumentException expected but not thrown");
18321         } catch (final IllegalArgumentException ignore) {
18322         }
18323         try {
18324             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18325                     true, new Matrix(1, 3),
18326                     this);
18327             fail("IllegalArgumentException expected but not thrown");
18328         } catch (final IllegalArgumentException ignore) {
18329         }
18330         assertNull(calibrator);
18331     }
18332 
18333     @Test
18334     public void testConstructor147() throws WrongSizeException {
18335         final Collection<StandardDeviationBodyKinematics> measurements =
18336                 Collections.emptyList();
18337 
18338         final Matrix ba = generateBa();
18339         final double[] bias = ba.getBuffer();
18340         final double biasX = ba.getElementAtIndex(0);
18341         final double biasY = ba.getElementAtIndex(1);
18342         final double biasZ = ba.getElementAtIndex(2);
18343 
18344         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18345         final double latitude = Math.toRadians(
18346                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18347         final double longitude = Math.toRadians(
18348                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18349         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18350         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18351         final NEDVelocity nedVelocity = new NEDVelocity();
18352         final ECEFPosition ecefPosition = new ECEFPosition();
18353         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18354         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18355                 ecefPosition, ecefVelocity);
18356 
18357         KnownPositionAccelerometerCalibrator calibrator =
18358                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
18359                         true, ba);
18360 
18361         // check default values
18362         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18363         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18364         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18365         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18366         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18367         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18368         final Acceleration bx2 = new Acceleration(0.0,
18369                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18370         calibrator.getInitialBiasXAsAcceleration(bx2);
18371         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18372         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18373         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18374         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18375         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18376         final Acceleration by2 = new Acceleration(0.0,
18377                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18378         calibrator.getInitialBiasYAsAcceleration(by2);
18379         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18380         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18381         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18382         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18383         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18384         final Acceleration bz2 = new Acceleration(0.0,
18385                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18386         calibrator.getInitialBiasZAsAcceleration(bz2);
18387         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18388         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18389         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18390         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18391         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18392         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18393         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18394         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18395         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18396         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18397         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18398         final double[] bias1 = calibrator.getInitialBias();
18399         assertArrayEquals(bias1, bias, 0.0);
18400         final double[] bias2 = new double[3];
18401         calibrator.getInitialBias(bias2);
18402         assertArrayEquals(bias1, bias2, 0.0);
18403         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18404         assertEquals(b1, ba);
18405         final Matrix b2 = new Matrix(3, 1);
18406         calibrator.getInitialBiasAsMatrix(b2);
18407         assertEquals(b1, b2);
18408         final Matrix ma1 = calibrator.getInitialMa();
18409         assertEquals(ma1, new Matrix(3, 3));
18410         final Matrix ma2 = new Matrix(3, 3);
18411         calibrator.getInitialMa(ma2);
18412         assertEquals(ma1, ma2);
18413         assertSame(calibrator.getEcefPosition(), ecefPosition);
18414         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18415         final NEDPosition nedPosition1 = new NEDPosition();
18416         assertTrue(calibrator.getNedPosition(nedPosition1));
18417         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18418         assertSame(calibrator.getMeasurements(), measurements);
18419         assertTrue(calibrator.isCommonAxisUsed());
18420         assertNull(calibrator.getListener());
18421         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18422         assertFalse(calibrator.isReady());
18423         assertFalse(calibrator.isRunning());
18424         assertNull(calibrator.getEstimatedBiases());
18425         assertFalse(calibrator.getEstimatedBiases(null));
18426         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18427         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18428         assertNull(calibrator.getEstimatedBiasFx());
18429         assertNull(calibrator.getEstimatedBiasFy());
18430         assertNull(calibrator.getEstimatedBiasFz());
18431         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18432         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18433         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18434         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18435         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18436         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18437         assertNull(calibrator.getEstimatedMa());
18438         assertNull(calibrator.getEstimatedSx());
18439         assertNull(calibrator.getEstimatedSy());
18440         assertNull(calibrator.getEstimatedSz());
18441         assertNull(calibrator.getEstimatedMxy());
18442         assertNull(calibrator.getEstimatedMxz());
18443         assertNull(calibrator.getEstimatedMyx());
18444         assertNull(calibrator.getEstimatedMyz());
18445         assertNull(calibrator.getEstimatedMzx());
18446         assertNull(calibrator.getEstimatedMzy());
18447         assertNull(calibrator.getEstimatedCovariance());
18448         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18449         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18450                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18451         assertNotNull(calibrator.getGroundTruthGravityNorm());
18452         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18453         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18454         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18455                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18456         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18457         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18458         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18459 
18460         // Force IllegalArgumentException
18461         calibrator = null;
18462         try {
18463             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18464                     measurements, true,
18465                     new Matrix(1, 1));
18466             fail("IllegalArgumentException expected but not thrown");
18467         } catch (final IllegalArgumentException ignore) {
18468         }
18469         try {
18470             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18471                     measurements, true,
18472                     new Matrix(1, 3));
18473             fail("IllegalArgumentException expected but not thrown");
18474         } catch (final IllegalArgumentException ignore) {
18475         }
18476         assertNull(calibrator);
18477     }
18478 
18479     @Test
18480     public void testConstructor148() throws WrongSizeException {
18481         final Collection<StandardDeviationBodyKinematics> measurements =
18482                 Collections.emptyList();
18483 
18484         final Matrix ba = generateBa();
18485         final double[] bias = ba.getBuffer();
18486         final double biasX = ba.getElementAtIndex(0);
18487         final double biasY = ba.getElementAtIndex(1);
18488         final double biasZ = ba.getElementAtIndex(2);
18489 
18490         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18491         final double latitude = Math.toRadians(
18492                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18493         final double longitude = Math.toRadians(
18494                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18495         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18496         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18497         final NEDVelocity nedVelocity = new NEDVelocity();
18498         final ECEFPosition ecefPosition = new ECEFPosition();
18499         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18500         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18501                 ecefPosition, ecefVelocity);
18502 
18503         KnownPositionAccelerometerCalibrator calibrator =
18504                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
18505                         true, ba, this);
18506 
18507         // check default values
18508         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18509         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18510         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18511         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18512         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18513         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18514         final Acceleration bx2 = new Acceleration(0.0,
18515                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18516         calibrator.getInitialBiasXAsAcceleration(bx2);
18517         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18518         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18519         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18520         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18521         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18522         final Acceleration by2 = new Acceleration(0.0,
18523                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18524         calibrator.getInitialBiasYAsAcceleration(by2);
18525         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18526         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18527         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18528         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18529         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18530         final Acceleration bz2 = new Acceleration(0.0,
18531                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18532         calibrator.getInitialBiasZAsAcceleration(bz2);
18533         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18534         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18535         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18536         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18537         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18538         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18539         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18540         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18541         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18542         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18543         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18544         final double[] bias1 = calibrator.getInitialBias();
18545         assertArrayEquals(bias1, bias, 0.0);
18546         final double[] bias2 = new double[3];
18547         calibrator.getInitialBias(bias2);
18548         assertArrayEquals(bias1, bias2, 0.0);
18549         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18550         assertEquals(b1, ba);
18551         final Matrix b2 = new Matrix(3, 1);
18552         calibrator.getInitialBiasAsMatrix(b2);
18553         assertEquals(b1, b2);
18554         final Matrix ma1 = calibrator.getInitialMa();
18555         assertEquals(ma1, new Matrix(3, 3));
18556         final Matrix ma2 = new Matrix(3, 3);
18557         calibrator.getInitialMa(ma2);
18558         assertEquals(ma1, ma2);
18559         assertSame(calibrator.getEcefPosition(), ecefPosition);
18560         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18561         final NEDPosition nedPosition1 = new NEDPosition();
18562         assertTrue(calibrator.getNedPosition(nedPosition1));
18563         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18564         assertSame(calibrator.getMeasurements(), measurements);
18565         assertTrue(calibrator.isCommonAxisUsed());
18566         assertSame(calibrator.getListener(), this);
18567         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18568         assertFalse(calibrator.isReady());
18569         assertFalse(calibrator.isRunning());
18570         assertNull(calibrator.getEstimatedBiases());
18571         assertFalse(calibrator.getEstimatedBiases(null));
18572         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18573         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18574         assertNull(calibrator.getEstimatedBiasFx());
18575         assertNull(calibrator.getEstimatedBiasFy());
18576         assertNull(calibrator.getEstimatedBiasFz());
18577         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18578         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18579         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18580         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18581         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18582         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18583         assertNull(calibrator.getEstimatedMa());
18584         assertNull(calibrator.getEstimatedSx());
18585         assertNull(calibrator.getEstimatedSy());
18586         assertNull(calibrator.getEstimatedSz());
18587         assertNull(calibrator.getEstimatedMxy());
18588         assertNull(calibrator.getEstimatedMxz());
18589         assertNull(calibrator.getEstimatedMyx());
18590         assertNull(calibrator.getEstimatedMyz());
18591         assertNull(calibrator.getEstimatedMzx());
18592         assertNull(calibrator.getEstimatedMzy());
18593         assertNull(calibrator.getEstimatedCovariance());
18594         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18595         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18596                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18597         assertNotNull(calibrator.getGroundTruthGravityNorm());
18598         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18599         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18600         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18601                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18602         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18603         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18604         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18605 
18606         // Force IllegalArgumentException
18607         calibrator = null;
18608         try {
18609             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18610                     measurements, true,
18611                     new Matrix(1, 1), this);
18612             fail("IllegalArgumentException expected but not thrown");
18613         } catch (final IllegalArgumentException ignore) {
18614         }
18615         try {
18616             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18617                     measurements, true,
18618                     new Matrix(1, 3), this);
18619             fail("IllegalArgumentException expected but not thrown");
18620         } catch (final IllegalArgumentException ignore) {
18621         }
18622         assertNull(calibrator);
18623     }
18624 
18625     @Test
18626     public void testConstructor149() throws WrongSizeException {
18627         final Matrix ba = generateBa();
18628         final double[] bias = ba.getBuffer();
18629         final double biasX = ba.getElementAtIndex(0);
18630         final double biasY = ba.getElementAtIndex(1);
18631         final double biasZ = ba.getElementAtIndex(2);
18632 
18633         final Matrix ma = generateMaCommonAxis();
18634         final double sx = ma.getElementAt(0, 0);
18635         final double sy = ma.getElementAt(1, 1);
18636         final double sz = ma.getElementAt(2, 2);
18637         final double mxy = ma.getElementAt(0, 1);
18638         final double mxz = ma.getElementAt(0, 2);
18639         final double myx = ma.getElementAt(1, 0);
18640         final double myz = ma.getElementAt(1, 2);
18641         final double mzx = ma.getElementAt(2, 0);
18642         final double mzy = ma.getElementAt(2, 1);
18643 
18644         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18645         final double latitude = Math.toRadians(
18646                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18647         final double longitude = Math.toRadians(
18648                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18649         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18650         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18651         final NEDVelocity nedVelocity = new NEDVelocity();
18652         final ECEFPosition ecefPosition = new ECEFPosition();
18653         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18654         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18655                 ecefPosition, ecefVelocity);
18656 
18657         KnownPositionAccelerometerCalibrator calibrator =
18658                 new KnownPositionAccelerometerCalibrator(ecefPosition, ba, ma);
18659 
18660         // check default values
18661         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18662         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18663         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18664         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18665         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18666         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18667         final Acceleration bx2 = new Acceleration(0.0,
18668                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18669         calibrator.getInitialBiasXAsAcceleration(bx2);
18670         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18671         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18672         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18673         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18674         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18675         final Acceleration by2 = new Acceleration(0.0,
18676                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18677         calibrator.getInitialBiasYAsAcceleration(by2);
18678         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18679         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18680         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18681         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18682         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18683         final Acceleration bz2 = new Acceleration(0.0,
18684                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18685         calibrator.getInitialBiasZAsAcceleration(bz2);
18686         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18687         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18688         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18689         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18690         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18691         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18692         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18693         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18694         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18695         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18696         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18697         final double[] bias1 = calibrator.getInitialBias();
18698         assertArrayEquals(bias1, bias, 0.0);
18699         final double[] bias2 = new double[3];
18700         calibrator.getInitialBias(bias2);
18701         assertArrayEquals(bias1, bias2, 0.0);
18702         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18703         assertEquals(b1, ba);
18704         final Matrix b2 = new Matrix(3, 1);
18705         calibrator.getInitialBiasAsMatrix(b2);
18706         assertEquals(b1, b2);
18707         final Matrix ma1 = new Matrix(3, 3);
18708         ma1.setSubmatrix(0, 0,
18709                 2, 2,
18710                 new double[]{sx, myx, mzx,
18711                         mxy, sy, mzy,
18712                         mxz, myz, sz});
18713         assertEquals(calibrator.getInitialMa(), ma1);
18714         final Matrix ma2 = new Matrix(3, 3);
18715         calibrator.getInitialMa(ma2);
18716         assertEquals(ma1, ma2);
18717         assertSame(calibrator.getEcefPosition(), ecefPosition);
18718         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18719         final NEDPosition nedPosition1 = new NEDPosition();
18720         assertTrue(calibrator.getNedPosition(nedPosition1));
18721         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18722         assertNull(calibrator.getMeasurements());
18723         assertFalse(calibrator.isCommonAxisUsed());
18724         assertNull(calibrator.getListener());
18725         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18726         assertFalse(calibrator.isReady());
18727         assertFalse(calibrator.isRunning());
18728         assertNull(calibrator.getEstimatedBiases());
18729         assertFalse(calibrator.getEstimatedBiases(null));
18730         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18731         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18732         assertNull(calibrator.getEstimatedBiasFx());
18733         assertNull(calibrator.getEstimatedBiasFy());
18734         assertNull(calibrator.getEstimatedBiasFz());
18735         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18736         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18737         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18738         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18739         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18740         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18741         assertNull(calibrator.getEstimatedMa());
18742         assertNull(calibrator.getEstimatedSx());
18743         assertNull(calibrator.getEstimatedSy());
18744         assertNull(calibrator.getEstimatedSz());
18745         assertNull(calibrator.getEstimatedMxy());
18746         assertNull(calibrator.getEstimatedMxz());
18747         assertNull(calibrator.getEstimatedMyx());
18748         assertNull(calibrator.getEstimatedMyz());
18749         assertNull(calibrator.getEstimatedMzx());
18750         assertNull(calibrator.getEstimatedMzy());
18751         assertNull(calibrator.getEstimatedCovariance());
18752         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18753         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18754                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18755         assertNotNull(calibrator.getGroundTruthGravityNorm());
18756         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18757         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18758         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18759                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18760         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18761         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18762         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18763 
18764         // Force IllegalArgumentException
18765         calibrator = null;
18766         try {
18767             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18768                     new Matrix(1, 1), ma);
18769             fail("IllegalArgumentException expected but not thrown");
18770         } catch (final IllegalArgumentException ignore) {
18771         }
18772         try {
18773             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18774                     new Matrix(1, 3), ma);
18775             fail("IllegalArgumentException expected but not thrown");
18776         } catch (final IllegalArgumentException ignore) {
18777         }
18778         try {
18779             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18780                     ba, new Matrix(1, 3));
18781             fail("IllegalArgumentException expected but not thrown");
18782         } catch (final IllegalArgumentException ignore) {
18783         }
18784         try {
18785             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18786                     ba, new Matrix(3, 1));
18787             fail("IllegalArgumentException expected but not thrown");
18788         } catch (final IllegalArgumentException ignore) {
18789         }
18790         assertNull(calibrator);
18791     }
18792 
18793     @Test
18794     public void testConstructor150() throws WrongSizeException {
18795         final Matrix ba = generateBa();
18796         final double[] bias = ba.getBuffer();
18797         final double biasX = ba.getElementAtIndex(0);
18798         final double biasY = ba.getElementAtIndex(1);
18799         final double biasZ = ba.getElementAtIndex(2);
18800 
18801         final Matrix ma = generateMaCommonAxis();
18802         final double sx = ma.getElementAt(0, 0);
18803         final double sy = ma.getElementAt(1, 1);
18804         final double sz = ma.getElementAt(2, 2);
18805         final double mxy = ma.getElementAt(0, 1);
18806         final double mxz = ma.getElementAt(0, 2);
18807         final double myx = ma.getElementAt(1, 0);
18808         final double myz = ma.getElementAt(1, 2);
18809         final double mzx = ma.getElementAt(2, 0);
18810         final double mzy = ma.getElementAt(2, 1);
18811 
18812         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18813         final double latitude = Math.toRadians(
18814                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18815         final double longitude = Math.toRadians(
18816                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18817         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18818         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18819         final NEDVelocity nedVelocity = new NEDVelocity();
18820         final ECEFPosition ecefPosition = new ECEFPosition();
18821         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18822         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18823                 ecefPosition, ecefVelocity);
18824 
18825         KnownPositionAccelerometerCalibrator calibrator =
18826                 new KnownPositionAccelerometerCalibrator(ecefPosition,
18827                         ba, ma, this);
18828 
18829         // check default values
18830         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18831         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18832         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18833         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18834         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18835         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18836         final Acceleration bx2 = new Acceleration(0.0,
18837                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18838         calibrator.getInitialBiasXAsAcceleration(bx2);
18839         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18840         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18841         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18842         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18843         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18844         final Acceleration by2 = new Acceleration(0.0,
18845                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18846         calibrator.getInitialBiasYAsAcceleration(by2);
18847         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18848         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18849         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18850         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18851         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18852         final Acceleration bz2 = new Acceleration(0.0,
18853                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18854         calibrator.getInitialBiasZAsAcceleration(bz2);
18855         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18856         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18857         assertEquals(calibrator.getInitialSx(), sx, 0.0);
18858         assertEquals(calibrator.getInitialSy(), sy, 0.0);
18859         assertEquals(calibrator.getInitialSz(), sz, 0.0);
18860         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18861         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18862         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18863         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18864         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18865         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18866         final double[] bias1 = calibrator.getInitialBias();
18867         assertArrayEquals(bias1, bias, 0.0);
18868         final double[] bias2 = new double[3];
18869         calibrator.getInitialBias(bias2);
18870         assertArrayEquals(bias1, bias2, 0.0);
18871         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18872         assertEquals(b1, ba);
18873         final Matrix b2 = new Matrix(3, 1);
18874         calibrator.getInitialBiasAsMatrix(b2);
18875         assertEquals(b1, b2);
18876         final Matrix ma1 = new Matrix(3, 3);
18877         ma1.setSubmatrix(0, 0,
18878                 2, 2,
18879                 new double[]{sx, myx, mzx,
18880                         mxy, sy, mzy,
18881                         mxz, myz, sz});
18882         assertEquals(calibrator.getInitialMa(), ma1);
18883         final Matrix ma2 = new Matrix(3, 3);
18884         calibrator.getInitialMa(ma2);
18885         assertEquals(ma1, ma2);
18886         assertSame(calibrator.getEcefPosition(), ecefPosition);
18887         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18888         final NEDPosition nedPosition1 = new NEDPosition();
18889         assertTrue(calibrator.getNedPosition(nedPosition1));
18890         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18891         assertNull(calibrator.getMeasurements());
18892         assertFalse(calibrator.isCommonAxisUsed());
18893         assertSame(calibrator.getListener(), this);
18894         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18895         assertFalse(calibrator.isReady());
18896         assertFalse(calibrator.isRunning());
18897         assertNull(calibrator.getEstimatedBiases());
18898         assertFalse(calibrator.getEstimatedBiases(null));
18899         assertNull(calibrator.getEstimatedBiasesAsMatrix());
18900         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18901         assertNull(calibrator.getEstimatedBiasFx());
18902         assertNull(calibrator.getEstimatedBiasFy());
18903         assertNull(calibrator.getEstimatedBiasFz());
18904         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18905         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18906         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18907         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18908         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18909         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18910         assertNull(calibrator.getEstimatedMa());
18911         assertNull(calibrator.getEstimatedSx());
18912         assertNull(calibrator.getEstimatedSy());
18913         assertNull(calibrator.getEstimatedSz());
18914         assertNull(calibrator.getEstimatedMxy());
18915         assertNull(calibrator.getEstimatedMxz());
18916         assertNull(calibrator.getEstimatedMyx());
18917         assertNull(calibrator.getEstimatedMyz());
18918         assertNull(calibrator.getEstimatedMzx());
18919         assertNull(calibrator.getEstimatedMzy());
18920         assertNull(calibrator.getEstimatedCovariance());
18921         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18922         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18923                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18924         assertNotNull(calibrator.getGroundTruthGravityNorm());
18925         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18926         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18927         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18928                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18929         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18930         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18931         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18932 
18933         // Force IllegalArgumentException
18934         calibrator = null;
18935         try {
18936             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18937                     new Matrix(1, 1), ma, this);
18938             fail("IllegalArgumentException expected but not thrown");
18939         } catch (final IllegalArgumentException ignore) {
18940         }
18941         try {
18942             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18943                     new Matrix(1, 3), ma, this);
18944             fail("IllegalArgumentException expected but not thrown");
18945         } catch (final IllegalArgumentException ignore) {
18946         }
18947         try {
18948             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18949                     ba, new Matrix(1, 3), this);
18950             fail("IllegalArgumentException expected but not thrown");
18951         } catch (final IllegalArgumentException ignore) {
18952         }
18953         try {
18954             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18955                     ba, new Matrix(3, 1), this);
18956             fail("IllegalArgumentException expected but not thrown");
18957         } catch (final IllegalArgumentException ignore) {
18958         }
18959         assertNull(calibrator);
18960     }
18961 
18962     @Test
18963     public void testConstructor151() throws WrongSizeException {
18964         final Collection<StandardDeviationBodyKinematics> measurements =
18965                 Collections.emptyList();
18966 
18967         final Matrix ba = generateBa();
18968         final double[] bias = ba.getBuffer();
18969         final double biasX = ba.getElementAtIndex(0);
18970         final double biasY = ba.getElementAtIndex(1);
18971         final double biasZ = ba.getElementAtIndex(2);
18972 
18973         final Matrix ma = generateMaCommonAxis();
18974         final double sx = ma.getElementAt(0, 0);
18975         final double sy = ma.getElementAt(1, 1);
18976         final double sz = ma.getElementAt(2, 2);
18977         final double mxy = ma.getElementAt(0, 1);
18978         final double mxz = ma.getElementAt(0, 2);
18979         final double myx = ma.getElementAt(1, 0);
18980         final double myz = ma.getElementAt(1, 2);
18981         final double mzx = ma.getElementAt(2, 0);
18982         final double mzy = ma.getElementAt(2, 1);
18983 
18984         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18985         final double latitude = Math.toRadians(
18986                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18987         final double longitude = Math.toRadians(
18988                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18989         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18990         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18991         final NEDVelocity nedVelocity = new NEDVelocity();
18992         final ECEFPosition ecefPosition = new ECEFPosition();
18993         final ECEFVelocity ecefVelocity = new ECEFVelocity();
18994         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18995                 ecefPosition, ecefVelocity);
18996 
18997         KnownPositionAccelerometerCalibrator calibrator =
18998                 new KnownPositionAccelerometerCalibrator(ecefPosition,
18999                         measurements, ba, ma);
19000 
19001         // check default values
19002         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19003         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19004         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19005         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19006         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19007         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19008         final Acceleration bx2 = new Acceleration(0.0,
19009                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19010         calibrator.getInitialBiasXAsAcceleration(bx2);
19011         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19012         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19013         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19014         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19015         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19016         final Acceleration by2 = new Acceleration(0.0,
19017                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19018         calibrator.getInitialBiasYAsAcceleration(by2);
19019         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19020         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19021         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19022         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19023         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19024         final Acceleration bz2 = new Acceleration(0.0,
19025                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19026         calibrator.getInitialBiasZAsAcceleration(bz2);
19027         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19028         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19029         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19030         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19031         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19032         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19033         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19034         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19035         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19036         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19037         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19038         final double[] bias1 = calibrator.getInitialBias();
19039         assertArrayEquals(bias1, bias, 0.0);
19040         final double[] bias2 = new double[3];
19041         calibrator.getInitialBias(bias2);
19042         assertArrayEquals(bias1, bias2, 0.0);
19043         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19044         assertEquals(b1, ba);
19045         final Matrix b2 = new Matrix(3, 1);
19046         calibrator.getInitialBiasAsMatrix(b2);
19047         assertEquals(b1, b2);
19048         final Matrix ma1 = new Matrix(3, 3);
19049         ma1.setSubmatrix(0, 0,
19050                 2, 2,
19051                 new double[]{sx, myx, mzx,
19052                         mxy, sy, mzy,
19053                         mxz, myz, sz});
19054         assertEquals(calibrator.getInitialMa(), ma1);
19055         final Matrix ma2 = new Matrix(3, 3);
19056         calibrator.getInitialMa(ma2);
19057         assertEquals(ma1, ma2);
19058         assertSame(calibrator.getEcefPosition(), ecefPosition);
19059         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19060         final NEDPosition nedPosition1 = new NEDPosition();
19061         assertTrue(calibrator.getNedPosition(nedPosition1));
19062         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19063         assertSame(calibrator.getMeasurements(), measurements);
19064         assertFalse(calibrator.isCommonAxisUsed());
19065         assertNull(calibrator.getListener());
19066         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19067         assertFalse(calibrator.isReady());
19068         assertFalse(calibrator.isRunning());
19069         assertNull(calibrator.getEstimatedBiases());
19070         assertFalse(calibrator.getEstimatedBiases(null));
19071         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19072         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19073         assertNull(calibrator.getEstimatedBiasFx());
19074         assertNull(calibrator.getEstimatedBiasFy());
19075         assertNull(calibrator.getEstimatedBiasFz());
19076         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19077         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19078         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19079         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19080         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19081         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19082         assertNull(calibrator.getEstimatedMa());
19083         assertNull(calibrator.getEstimatedSx());
19084         assertNull(calibrator.getEstimatedSy());
19085         assertNull(calibrator.getEstimatedSz());
19086         assertNull(calibrator.getEstimatedMxy());
19087         assertNull(calibrator.getEstimatedMxz());
19088         assertNull(calibrator.getEstimatedMyx());
19089         assertNull(calibrator.getEstimatedMyz());
19090         assertNull(calibrator.getEstimatedMzx());
19091         assertNull(calibrator.getEstimatedMzy());
19092         assertNull(calibrator.getEstimatedCovariance());
19093         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19094         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19095                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19096         assertNotNull(calibrator.getGroundTruthGravityNorm());
19097         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19098         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19099         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19100                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19101         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19102         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19103         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19104 
19105         // Force IllegalArgumentException
19106         calibrator = null;
19107         try {
19108             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19109                     measurements, new Matrix(1, 1), ma);
19110             fail("IllegalArgumentException expected but not thrown");
19111         } catch (final IllegalArgumentException ignore) {
19112         }
19113         try {
19114             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19115                     measurements, new Matrix(1, 3), ma);
19116             fail("IllegalArgumentException expected but not thrown");
19117         } catch (final IllegalArgumentException ignore) {
19118         }
19119         try {
19120             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19121                     measurements, ba, new Matrix(1, 3));
19122             fail("IllegalArgumentException expected but not thrown");
19123         } catch (final IllegalArgumentException ignore) {
19124         }
19125         try {
19126             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19127                     measurements, ba, new Matrix(3, 1));
19128             fail("IllegalArgumentException expected but not thrown");
19129         } catch (final IllegalArgumentException ignore) {
19130         }
19131         assertNull(calibrator);
19132     }
19133 
19134     @Test
19135     public void testConstructor152() throws WrongSizeException {
19136         final Collection<StandardDeviationBodyKinematics> measurements =
19137                 Collections.emptyList();
19138 
19139         final Matrix ba = generateBa();
19140         final double[] bias = ba.getBuffer();
19141         final double biasX = ba.getElementAtIndex(0);
19142         final double biasY = ba.getElementAtIndex(1);
19143         final double biasZ = ba.getElementAtIndex(2);
19144 
19145         final Matrix ma = generateMaCommonAxis();
19146         final double sx = ma.getElementAt(0, 0);
19147         final double sy = ma.getElementAt(1, 1);
19148         final double sz = ma.getElementAt(2, 2);
19149         final double mxy = ma.getElementAt(0, 1);
19150         final double mxz = ma.getElementAt(0, 2);
19151         final double myx = ma.getElementAt(1, 0);
19152         final double myz = ma.getElementAt(1, 2);
19153         final double mzx = ma.getElementAt(2, 0);
19154         final double mzy = ma.getElementAt(2, 1);
19155 
19156         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19157         final double latitude = Math.toRadians(
19158                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19159         final double longitude = Math.toRadians(
19160                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19161         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19162         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19163         final NEDVelocity nedVelocity = new NEDVelocity();
19164         final ECEFPosition ecefPosition = new ECEFPosition();
19165         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19166         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19167                 ecefPosition, ecefVelocity);
19168 
19169         KnownPositionAccelerometerCalibrator calibrator =
19170                 new KnownPositionAccelerometerCalibrator(ecefPosition,
19171                         measurements, ba, ma, this);
19172 
19173         // check default values
19174         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19175         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19176         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19177         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19178         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19179         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19180         final Acceleration bx2 = new Acceleration(0.0,
19181                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19182         calibrator.getInitialBiasXAsAcceleration(bx2);
19183         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19184         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19185         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19186         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19187         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19188         final Acceleration by2 = new Acceleration(0.0,
19189                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19190         calibrator.getInitialBiasYAsAcceleration(by2);
19191         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19192         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19193         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19194         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19195         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19196         final Acceleration bz2 = new Acceleration(0.0,
19197                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19198         calibrator.getInitialBiasZAsAcceleration(bz2);
19199         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19200         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19201         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19202         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19203         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19204         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19205         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19206         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19207         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19208         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19209         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19210         final double[] bias1 = calibrator.getInitialBias();
19211         assertArrayEquals(bias1, bias, 0.0);
19212         final double[] bias2 = new double[3];
19213         calibrator.getInitialBias(bias2);
19214         assertArrayEquals(bias1, bias2, 0.0);
19215         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19216         assertEquals(b1, ba);
19217         final Matrix b2 = new Matrix(3, 1);
19218         calibrator.getInitialBiasAsMatrix(b2);
19219         assertEquals(b1, b2);
19220         final Matrix ma1 = new Matrix(3, 3);
19221         ma1.setSubmatrix(0, 0,
19222                 2, 2,
19223                 new double[]{sx, myx, mzx,
19224                         mxy, sy, mzy,
19225                         mxz, myz, sz});
19226         assertEquals(calibrator.getInitialMa(), ma1);
19227         final Matrix ma2 = new Matrix(3, 3);
19228         calibrator.getInitialMa(ma2);
19229         assertEquals(ma1, ma2);
19230         assertSame(calibrator.getEcefPosition(), ecefPosition);
19231         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19232         final NEDPosition nedPosition1 = new NEDPosition();
19233         assertTrue(calibrator.getNedPosition(nedPosition1));
19234         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19235         assertSame(calibrator.getMeasurements(), measurements);
19236         assertFalse(calibrator.isCommonAxisUsed());
19237         assertSame(calibrator.getListener(), this);
19238         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19239         assertFalse(calibrator.isReady());
19240         assertFalse(calibrator.isRunning());
19241         assertNull(calibrator.getEstimatedBiases());
19242         assertFalse(calibrator.getEstimatedBiases(null));
19243         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19244         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19245         assertNull(calibrator.getEstimatedBiasFx());
19246         assertNull(calibrator.getEstimatedBiasFy());
19247         assertNull(calibrator.getEstimatedBiasFz());
19248         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19249         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19250         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19251         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19252         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19253         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19254         assertNull(calibrator.getEstimatedMa());
19255         assertNull(calibrator.getEstimatedSx());
19256         assertNull(calibrator.getEstimatedSy());
19257         assertNull(calibrator.getEstimatedSz());
19258         assertNull(calibrator.getEstimatedMxy());
19259         assertNull(calibrator.getEstimatedMxz());
19260         assertNull(calibrator.getEstimatedMyx());
19261         assertNull(calibrator.getEstimatedMyz());
19262         assertNull(calibrator.getEstimatedMzx());
19263         assertNull(calibrator.getEstimatedMzy());
19264         assertNull(calibrator.getEstimatedCovariance());
19265         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19266         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19267                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19268         assertNotNull(calibrator.getGroundTruthGravityNorm());
19269         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19270         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19271         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19272                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19273         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19274         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19275         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19276 
19277         // Force IllegalArgumentException
19278         calibrator = null;
19279         try {
19280             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19281                     measurements, new Matrix(1, 1), ma, this);
19282             fail("IllegalArgumentException expected but not thrown");
19283         } catch (final IllegalArgumentException ignore) {
19284         }
19285         try {
19286             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19287                     measurements, new Matrix(1, 3), ma, this);
19288             fail("IllegalArgumentException expected but not thrown");
19289         } catch (final IllegalArgumentException ignore) {
19290         }
19291         try {
19292             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19293                     measurements, ba, new Matrix(1, 3), this);
19294             fail("IllegalArgumentException expected but not thrown");
19295         } catch (final IllegalArgumentException ignore) {
19296         }
19297         try {
19298             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19299                     measurements, ba, new Matrix(3, 1), this);
19300             fail("IllegalArgumentException expected but not thrown");
19301         } catch (final IllegalArgumentException ignore) {
19302         }
19303         assertNull(calibrator);
19304     }
19305 
19306     @Test
19307     public void testConstructor153() throws WrongSizeException {
19308         final Matrix ba = generateBa();
19309         final double[] bias = ba.getBuffer();
19310         final double biasX = ba.getElementAtIndex(0);
19311         final double biasY = ba.getElementAtIndex(1);
19312         final double biasZ = ba.getElementAtIndex(2);
19313 
19314         final Matrix ma = generateMaCommonAxis();
19315         final double sx = ma.getElementAt(0, 0);
19316         final double sy = ma.getElementAt(1, 1);
19317         final double sz = ma.getElementAt(2, 2);
19318         final double mxy = ma.getElementAt(0, 1);
19319         final double mxz = ma.getElementAt(0, 2);
19320         final double myx = ma.getElementAt(1, 0);
19321         final double myz = ma.getElementAt(1, 2);
19322         final double mzx = ma.getElementAt(2, 0);
19323         final double mzy = ma.getElementAt(2, 1);
19324 
19325         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19326         final double latitude = Math.toRadians(
19327                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19328         final double longitude = Math.toRadians(
19329                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19330         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19331         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19332         final NEDVelocity nedVelocity = new NEDVelocity();
19333         final ECEFPosition ecefPosition = new ECEFPosition();
19334         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19335         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19336                 ecefPosition, ecefVelocity);
19337 
19338         KnownPositionAccelerometerCalibrator calibrator =
19339                 new KnownPositionAccelerometerCalibrator(ecefPosition,
19340                         true, ba, ma);
19341 
19342         // check default values
19343         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19344         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19345         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19346         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19347         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19348         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19349         final Acceleration bx2 = new Acceleration(0.0,
19350                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19351         calibrator.getInitialBiasXAsAcceleration(bx2);
19352         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19353         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19354         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19355         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19356         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19357         final Acceleration by2 = new Acceleration(0.0,
19358                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19359         calibrator.getInitialBiasYAsAcceleration(by2);
19360         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19361         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19362         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19363         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19364         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19365         final Acceleration bz2 = new Acceleration(0.0,
19366                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19367         calibrator.getInitialBiasZAsAcceleration(bz2);
19368         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19369         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19370         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19371         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19372         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19373         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19374         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19375         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19376         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19377         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19378         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19379         final double[] bias1 = calibrator.getInitialBias();
19380         assertArrayEquals(bias1, bias, 0.0);
19381         final double[] bias2 = new double[3];
19382         calibrator.getInitialBias(bias2);
19383         assertArrayEquals(bias1, bias2, 0.0);
19384         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19385         assertEquals(b1, ba);
19386         final Matrix b2 = new Matrix(3, 1);
19387         calibrator.getInitialBiasAsMatrix(b2);
19388         assertEquals(b1, b2);
19389         final Matrix ma1 = new Matrix(3, 3);
19390         ma1.setSubmatrix(0, 0,
19391                 2, 2,
19392                 new double[]{sx, myx, mzx,
19393                         mxy, sy, mzy,
19394                         mxz, myz, sz});
19395         assertEquals(calibrator.getInitialMa(), ma1);
19396         final Matrix ma2 = new Matrix(3, 3);
19397         calibrator.getInitialMa(ma2);
19398         assertEquals(ma1, ma2);
19399         assertSame(calibrator.getEcefPosition(), ecefPosition);
19400         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19401         final NEDPosition nedPosition1 = new NEDPosition();
19402         assertTrue(calibrator.getNedPosition(nedPosition1));
19403         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19404         assertNull(calibrator.getMeasurements());
19405         assertTrue(calibrator.isCommonAxisUsed());
19406         assertNull(calibrator.getListener());
19407         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19408         assertFalse(calibrator.isReady());
19409         assertFalse(calibrator.isRunning());
19410         assertNull(calibrator.getEstimatedBiases());
19411         assertFalse(calibrator.getEstimatedBiases(null));
19412         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19413         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19414         assertNull(calibrator.getEstimatedBiasFx());
19415         assertNull(calibrator.getEstimatedBiasFy());
19416         assertNull(calibrator.getEstimatedBiasFz());
19417         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19418         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19419         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19420         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19421         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19422         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19423         assertNull(calibrator.getEstimatedMa());
19424         assertNull(calibrator.getEstimatedSx());
19425         assertNull(calibrator.getEstimatedSy());
19426         assertNull(calibrator.getEstimatedSz());
19427         assertNull(calibrator.getEstimatedMxy());
19428         assertNull(calibrator.getEstimatedMxz());
19429         assertNull(calibrator.getEstimatedMyx());
19430         assertNull(calibrator.getEstimatedMyz());
19431         assertNull(calibrator.getEstimatedMzx());
19432         assertNull(calibrator.getEstimatedMzy());
19433         assertNull(calibrator.getEstimatedCovariance());
19434         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19435         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19436                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19437         assertNotNull(calibrator.getGroundTruthGravityNorm());
19438         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19439         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19440         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19441                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19442         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19443         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19444         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19445 
19446         // Force IllegalArgumentException
19447         calibrator = null;
19448         try {
19449             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19450                     true, new Matrix(1, 1), ma);
19451             fail("IllegalArgumentException expected but not thrown");
19452         } catch (final IllegalArgumentException ignore) {
19453         }
19454         try {
19455             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19456                     true, new Matrix(1, 3), ma);
19457             fail("IllegalArgumentException expected but not thrown");
19458         } catch (final IllegalArgumentException ignore) {
19459         }
19460         try {
19461             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19462                     true, ba, new Matrix(1, 3));
19463             fail("IllegalArgumentException expected but not thrown");
19464         } catch (final IllegalArgumentException ignore) {
19465         }
19466         try {
19467             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19468                     true, ba, new Matrix(3, 1));
19469             fail("IllegalArgumentException expected but not thrown");
19470         } catch (final IllegalArgumentException ignore) {
19471         }
19472         assertNull(calibrator);
19473     }
19474 
19475     @Test
19476     public void testConstructor154() throws WrongSizeException {
19477         final Matrix ba = generateBa();
19478         final double[] bias = ba.getBuffer();
19479         final double biasX = ba.getElementAtIndex(0);
19480         final double biasY = ba.getElementAtIndex(1);
19481         final double biasZ = ba.getElementAtIndex(2);
19482 
19483         final Matrix ma = generateMaCommonAxis();
19484         final double sx = ma.getElementAt(0, 0);
19485         final double sy = ma.getElementAt(1, 1);
19486         final double sz = ma.getElementAt(2, 2);
19487         final double mxy = ma.getElementAt(0, 1);
19488         final double mxz = ma.getElementAt(0, 2);
19489         final double myx = ma.getElementAt(1, 0);
19490         final double myz = ma.getElementAt(1, 2);
19491         final double mzx = ma.getElementAt(2, 0);
19492         final double mzy = ma.getElementAt(2, 1);
19493 
19494         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19495         final double latitude = Math.toRadians(
19496                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19497         final double longitude = Math.toRadians(
19498                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19499         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19500         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19501         final NEDVelocity nedVelocity = new NEDVelocity();
19502         final ECEFPosition ecefPosition = new ECEFPosition();
19503         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19504         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19505                 ecefPosition, ecefVelocity);
19506 
19507         KnownPositionAccelerometerCalibrator calibrator =
19508                 new KnownPositionAccelerometerCalibrator(ecefPosition,
19509                         true, ba, ma, this);
19510 
19511         // check default values
19512         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19513         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19514         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19515         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19516         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19517         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19518         final Acceleration bx2 = new Acceleration(0.0,
19519                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19520         calibrator.getInitialBiasXAsAcceleration(bx2);
19521         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19522         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19523         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19524         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19525         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19526         final Acceleration by2 = new Acceleration(0.0,
19527                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19528         calibrator.getInitialBiasYAsAcceleration(by2);
19529         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19530         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19531         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19532         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19533         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19534         final Acceleration bz2 = new Acceleration(0.0,
19535                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19536         calibrator.getInitialBiasZAsAcceleration(bz2);
19537         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19538         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19539         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19540         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19541         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19542         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19543         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19544         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19545         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19546         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19547         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19548         final double[] bias1 = calibrator.getInitialBias();
19549         assertArrayEquals(bias1, bias, 0.0);
19550         final double[] bias2 = new double[3];
19551         calibrator.getInitialBias(bias2);
19552         assertArrayEquals(bias1, bias2, 0.0);
19553         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19554         assertEquals(b1, ba);
19555         final Matrix b2 = new Matrix(3, 1);
19556         calibrator.getInitialBiasAsMatrix(b2);
19557         assertEquals(b1, b2);
19558         final Matrix ma1 = new Matrix(3, 3);
19559         ma1.setSubmatrix(0, 0,
19560                 2, 2,
19561                 new double[]{sx, myx, mzx,
19562                         mxy, sy, mzy,
19563                         mxz, myz, sz});
19564         assertEquals(calibrator.getInitialMa(), ma1);
19565         final Matrix ma2 = new Matrix(3, 3);
19566         calibrator.getInitialMa(ma2);
19567         assertEquals(ma1, ma2);
19568         assertSame(calibrator.getEcefPosition(), ecefPosition);
19569         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19570         final NEDPosition nedPosition1 = new NEDPosition();
19571         assertTrue(calibrator.getNedPosition(nedPosition1));
19572         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19573         assertNull(calibrator.getMeasurements());
19574         assertTrue(calibrator.isCommonAxisUsed());
19575         assertSame(calibrator.getListener(), this);
19576         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19577         assertFalse(calibrator.isReady());
19578         assertFalse(calibrator.isRunning());
19579         assertNull(calibrator.getEstimatedBiases());
19580         assertFalse(calibrator.getEstimatedBiases(null));
19581         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19582         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19583         assertNull(calibrator.getEstimatedBiasFx());
19584         assertNull(calibrator.getEstimatedBiasFy());
19585         assertNull(calibrator.getEstimatedBiasFz());
19586         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19587         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19588         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19589         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19590         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19591         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19592         assertNull(calibrator.getEstimatedMa());
19593         assertNull(calibrator.getEstimatedSx());
19594         assertNull(calibrator.getEstimatedSy());
19595         assertNull(calibrator.getEstimatedSz());
19596         assertNull(calibrator.getEstimatedMxy());
19597         assertNull(calibrator.getEstimatedMxz());
19598         assertNull(calibrator.getEstimatedMyx());
19599         assertNull(calibrator.getEstimatedMyz());
19600         assertNull(calibrator.getEstimatedMzx());
19601         assertNull(calibrator.getEstimatedMzy());
19602         assertNull(calibrator.getEstimatedCovariance());
19603         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19604         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19605                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19606         assertNotNull(calibrator.getGroundTruthGravityNorm());
19607         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19608         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19609         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19610                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19611         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19612         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19613         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19614 
19615         // Force IllegalArgumentException
19616         calibrator = null;
19617         try {
19618             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19619                     true, new Matrix(1, 1), ma, this);
19620             fail("IllegalArgumentException expected but not thrown");
19621         } catch (final IllegalArgumentException ignore) {
19622         }
19623         try {
19624             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19625                     true, new Matrix(1, 3), ma, this);
19626             fail("IllegalArgumentException expected but not thrown");
19627         } catch (final IllegalArgumentException ignore) {
19628         }
19629         try {
19630             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19631                     true, ba, new Matrix(1, 3), this);
19632             fail("IllegalArgumentException expected but not thrown");
19633         } catch (final IllegalArgumentException ignore) {
19634         }
19635         try {
19636             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19637                     true, ba, new Matrix(3, 1), this);
19638             fail("IllegalArgumentException expected but not thrown");
19639         } catch (final IllegalArgumentException ignore) {
19640         }
19641         assertNull(calibrator);
19642     }
19643 
19644     @Test
19645     public void testConstructor155() throws WrongSizeException {
19646         final Collection<StandardDeviationBodyKinematics> measurements =
19647                 Collections.emptyList();
19648 
19649         final Matrix ba = generateBa();
19650         final double[] bias = ba.getBuffer();
19651         final double biasX = ba.getElementAtIndex(0);
19652         final double biasY = ba.getElementAtIndex(1);
19653         final double biasZ = ba.getElementAtIndex(2);
19654 
19655         final Matrix ma = generateMaCommonAxis();
19656         final double sx = ma.getElementAt(0, 0);
19657         final double sy = ma.getElementAt(1, 1);
19658         final double sz = ma.getElementAt(2, 2);
19659         final double mxy = ma.getElementAt(0, 1);
19660         final double mxz = ma.getElementAt(0, 2);
19661         final double myx = ma.getElementAt(1, 0);
19662         final double myz = ma.getElementAt(1, 2);
19663         final double mzx = ma.getElementAt(2, 0);
19664         final double mzy = ma.getElementAt(2, 1);
19665 
19666         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19667         final double latitude = Math.toRadians(
19668                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19669         final double longitude = Math.toRadians(
19670                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19671         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19672         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19673         final NEDVelocity nedVelocity = new NEDVelocity();
19674         final ECEFPosition ecefPosition = new ECEFPosition();
19675         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19676         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19677                 ecefPosition, ecefVelocity);
19678 
19679         KnownPositionAccelerometerCalibrator calibrator =
19680                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
19681                         true, ba, ma);
19682 
19683         // check default values
19684         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19685         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19686         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19687         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19688         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19689         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19690         final Acceleration bx2 = new Acceleration(0.0,
19691                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19692         calibrator.getInitialBiasXAsAcceleration(bx2);
19693         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19694         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19695         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19696         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19697         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19698         final Acceleration by2 = new Acceleration(0.0,
19699                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19700         calibrator.getInitialBiasYAsAcceleration(by2);
19701         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19702         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19703         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19704         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19705         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19706         final Acceleration bz2 = new Acceleration(0.0,
19707                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19708         calibrator.getInitialBiasZAsAcceleration(bz2);
19709         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19710         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19711         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19712         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19713         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19714         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19715         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19716         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19717         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19718         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19719         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19720         final double[] bias1 = calibrator.getInitialBias();
19721         assertArrayEquals(bias1, bias, 0.0);
19722         final double[] bias2 = new double[3];
19723         calibrator.getInitialBias(bias2);
19724         assertArrayEquals(bias1, bias2, 0.0);
19725         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19726         assertEquals(b1, ba);
19727         final Matrix b2 = new Matrix(3, 1);
19728         calibrator.getInitialBiasAsMatrix(b2);
19729         assertEquals(b1, b2);
19730         final Matrix ma1 = new Matrix(3, 3);
19731         ma1.setSubmatrix(0, 0,
19732                 2, 2,
19733                 new double[]{sx, myx, mzx,
19734                         mxy, sy, mzy,
19735                         mxz, myz, sz});
19736         assertEquals(calibrator.getInitialMa(), ma1);
19737         final Matrix ma2 = new Matrix(3, 3);
19738         calibrator.getInitialMa(ma2);
19739         assertEquals(ma1, ma2);
19740         assertSame(calibrator.getEcefPosition(), ecefPosition);
19741         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19742         final NEDPosition nedPosition1 = new NEDPosition();
19743         assertTrue(calibrator.getNedPosition(nedPosition1));
19744         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19745         assertSame(calibrator.getMeasurements(), measurements);
19746         assertTrue(calibrator.isCommonAxisUsed());
19747         assertNull(calibrator.getListener());
19748         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19749         assertFalse(calibrator.isReady());
19750         assertFalse(calibrator.isRunning());
19751         assertNull(calibrator.getEstimatedBiases());
19752         assertFalse(calibrator.getEstimatedBiases(null));
19753         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19754         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19755         assertNull(calibrator.getEstimatedBiasFx());
19756         assertNull(calibrator.getEstimatedBiasFy());
19757         assertNull(calibrator.getEstimatedBiasFz());
19758         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19759         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19760         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19761         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19762         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19763         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19764         assertNull(calibrator.getEstimatedMa());
19765         assertNull(calibrator.getEstimatedSx());
19766         assertNull(calibrator.getEstimatedSy());
19767         assertNull(calibrator.getEstimatedSz());
19768         assertNull(calibrator.getEstimatedMxy());
19769         assertNull(calibrator.getEstimatedMxz());
19770         assertNull(calibrator.getEstimatedMyx());
19771         assertNull(calibrator.getEstimatedMyz());
19772         assertNull(calibrator.getEstimatedMzx());
19773         assertNull(calibrator.getEstimatedMzy());
19774         assertNull(calibrator.getEstimatedCovariance());
19775         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19776         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19777                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19778         assertNotNull(calibrator.getGroundTruthGravityNorm());
19779         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19780         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19781         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19782                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19783         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19784         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19785         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19786 
19787         // Force IllegalArgumentException
19788         calibrator = null;
19789         try {
19790             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19791                     measurements, true,
19792                     new Matrix(1, 1), ma);
19793             fail("IllegalArgumentException expected but not thrown");
19794         } catch (final IllegalArgumentException ignore) {
19795         }
19796         try {
19797             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19798                     measurements, true,
19799                     new Matrix(1, 3), ma);
19800             fail("IllegalArgumentException expected but not thrown");
19801         } catch (final IllegalArgumentException ignore) {
19802         }
19803         try {
19804             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19805                     measurements, true,
19806                     ba, new Matrix(1, 3));
19807             fail("IllegalArgumentException expected but not thrown");
19808         } catch (final IllegalArgumentException ignore) {
19809         }
19810         try {
19811             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19812                     measurements, true,
19813                     ba, new Matrix(3, 1));
19814             fail("IllegalArgumentException expected but not thrown");
19815         } catch (final IllegalArgumentException ignore) {
19816         }
19817         assertNull(calibrator);
19818     }
19819 
19820     @Test
19821     public void testConstructor156() throws WrongSizeException {
19822         final Collection<StandardDeviationBodyKinematics> measurements =
19823                 Collections.emptyList();
19824 
19825         final Matrix ba = generateBa();
19826         final double[] bias = ba.getBuffer();
19827         final double biasX = ba.getElementAtIndex(0);
19828         final double biasY = ba.getElementAtIndex(1);
19829         final double biasZ = ba.getElementAtIndex(2);
19830 
19831         final Matrix ma = generateMaCommonAxis();
19832         final double sx = ma.getElementAt(0, 0);
19833         final double sy = ma.getElementAt(1, 1);
19834         final double sz = ma.getElementAt(2, 2);
19835         final double mxy = ma.getElementAt(0, 1);
19836         final double mxz = ma.getElementAt(0, 2);
19837         final double myx = ma.getElementAt(1, 0);
19838         final double myz = ma.getElementAt(1, 2);
19839         final double mzx = ma.getElementAt(2, 0);
19840         final double mzy = ma.getElementAt(2, 1);
19841 
19842         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19843         final double latitude = Math.toRadians(
19844                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19845         final double longitude = Math.toRadians(
19846                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19847         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19848         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19849         final NEDVelocity nedVelocity = new NEDVelocity();
19850         final ECEFPosition ecefPosition = new ECEFPosition();
19851         final ECEFVelocity ecefVelocity = new ECEFVelocity();
19852         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19853                 ecefPosition, ecefVelocity);
19854 
19855         KnownPositionAccelerometerCalibrator calibrator =
19856                 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
19857                         true, ba, ma, this);
19858 
19859         // check default values
19860         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19861         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19862         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19863         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19864         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19865         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19866         final Acceleration bx2 = new Acceleration(0.0,
19867                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19868         calibrator.getInitialBiasXAsAcceleration(bx2);
19869         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19870         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19871         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19872         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19873         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19874         final Acceleration by2 = new Acceleration(0.0,
19875                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19876         calibrator.getInitialBiasYAsAcceleration(by2);
19877         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19878         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19879         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19880         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19881         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19882         final Acceleration bz2 = new Acceleration(0.0,
19883                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19884         calibrator.getInitialBiasZAsAcceleration(bz2);
19885         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19886         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19887         assertEquals(calibrator.getInitialSx(), sx, 0.0);
19888         assertEquals(calibrator.getInitialSy(), sy, 0.0);
19889         assertEquals(calibrator.getInitialSz(), sz, 0.0);
19890         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19891         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19892         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19893         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19894         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19895         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19896         final double[] bias1 = calibrator.getInitialBias();
19897         assertArrayEquals(bias1, bias, 0.0);
19898         final double[] bias2 = new double[3];
19899         calibrator.getInitialBias(bias2);
19900         assertArrayEquals(bias1, bias2, 0.0);
19901         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19902         assertEquals(b1, ba);
19903         final Matrix b2 = new Matrix(3, 1);
19904         calibrator.getInitialBiasAsMatrix(b2);
19905         assertEquals(b1, b2);
19906         final Matrix ma1 = new Matrix(3, 3);
19907         ma1.setSubmatrix(0, 0,
19908                 2, 2,
19909                 new double[]{sx, myx, mzx,
19910                         mxy, sy, mzy,
19911                         mxz, myz, sz});
19912         assertEquals(calibrator.getInitialMa(), ma1);
19913         final Matrix ma2 = new Matrix(3, 3);
19914         calibrator.getInitialMa(ma2);
19915         assertEquals(ma1, ma2);
19916         assertSame(calibrator.getEcefPosition(), ecefPosition);
19917         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19918         final NEDPosition nedPosition1 = new NEDPosition();
19919         assertTrue(calibrator.getNedPosition(nedPosition1));
19920         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19921         assertSame(calibrator.getMeasurements(), measurements);
19922         assertTrue(calibrator.isCommonAxisUsed());
19923         assertSame(calibrator.getListener(), this);
19924         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19925         assertFalse(calibrator.isReady());
19926         assertFalse(calibrator.isRunning());
19927         assertNull(calibrator.getEstimatedBiases());
19928         assertFalse(calibrator.getEstimatedBiases(null));
19929         assertNull(calibrator.getEstimatedBiasesAsMatrix());
19930         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19931         assertNull(calibrator.getEstimatedBiasFx());
19932         assertNull(calibrator.getEstimatedBiasFy());
19933         assertNull(calibrator.getEstimatedBiasFz());
19934         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19935         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19936         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19937         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19938         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19939         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19940         assertNull(calibrator.getEstimatedMa());
19941         assertNull(calibrator.getEstimatedSx());
19942         assertNull(calibrator.getEstimatedSy());
19943         assertNull(calibrator.getEstimatedSz());
19944         assertNull(calibrator.getEstimatedMxy());
19945         assertNull(calibrator.getEstimatedMxz());
19946         assertNull(calibrator.getEstimatedMyx());
19947         assertNull(calibrator.getEstimatedMyz());
19948         assertNull(calibrator.getEstimatedMzx());
19949         assertNull(calibrator.getEstimatedMzy());
19950         assertNull(calibrator.getEstimatedCovariance());
19951         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19952         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19953                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19954         assertNotNull(calibrator.getGroundTruthGravityNorm());
19955         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19956         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19957         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19958                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19959         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19960         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19961         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19962 
19963         // Force IllegalArgumentException
19964         calibrator = null;
19965         try {
19966             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19967                     measurements, true,
19968                     new Matrix(1, 1), ma, this);
19969             fail("IllegalArgumentException expected but not thrown");
19970         } catch (final IllegalArgumentException ignore) {
19971         }
19972         try {
19973             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19974                     measurements, true,
19975                     new Matrix(1, 3), ma, this);
19976             fail("IllegalArgumentException expected but not thrown");
19977         } catch (final IllegalArgumentException ignore) {
19978         }
19979         try {
19980             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19981                     measurements, true,
19982                     ba, new Matrix(1, 3), this);
19983             fail("IllegalArgumentException expected but not thrown");
19984         } catch (final IllegalArgumentException ignore) {
19985         }
19986         try {
19987             calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19988                     measurements, true,
19989                     ba, new Matrix(3, 1), this);
19990             fail("IllegalArgumentException expected but not thrown");
19991         } catch (final IllegalArgumentException ignore) {
19992         }
19993         assertNull(calibrator);
19994     }
19995 
19996     @Test
19997     public void testConstructor157() throws WrongSizeException {
19998         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19999         final double latitude = Math.toRadians(
20000                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20001         final double longitude = Math.toRadians(
20002                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20003         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20004         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20005         final NEDVelocity nedVelocity = new NEDVelocity();
20006         final ECEFPosition ecefPosition = new ECEFPosition();
20007         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20008         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20009                 ecefPosition, ecefVelocity);
20010 
20011         final KnownPositionAccelerometerCalibrator calibrator =
20012                 new KnownPositionAccelerometerCalibrator(nedPosition);
20013 
20014         // check default values
20015         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20016         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20017         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20018         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20019         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20020         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20021         final Acceleration bx2 = new Acceleration(0.0,
20022                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20023         calibrator.getInitialBiasXAsAcceleration(bx2);
20024         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20025         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20026         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20027         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20028         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20029         final Acceleration by2 = new Acceleration(0.0,
20030                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20031         calibrator.getInitialBiasYAsAcceleration(by2);
20032         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20033         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20034         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20035         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20036         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20037         final Acceleration bz2 = new Acceleration(0.0,
20038                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20039         calibrator.getInitialBiasZAsAcceleration(bz2);
20040         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20041         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20042         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20043         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20044         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20045         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20046         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20047         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20048         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20049         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20050         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20051         final double[] bias1 = calibrator.getInitialBias();
20052         assertArrayEquals(bias1, new double[3], 0.0);
20053         final double[] bias2 = new double[3];
20054         calibrator.getInitialBias(bias2);
20055         assertArrayEquals(bias1, bias2, 0.0);
20056         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20057         assertEquals(b1, new Matrix(3, 1));
20058         final Matrix b2 = new Matrix(3, 1);
20059         calibrator.getInitialBiasAsMatrix(b2);
20060         assertEquals(b1, b2);
20061         final Matrix ma1 = calibrator.getInitialMa();
20062         assertEquals(ma1, new Matrix(3, 3));
20063         final Matrix ma2 = new Matrix(3, 3);
20064         calibrator.getInitialMa(ma2);
20065         assertEquals(ma1, ma2);
20066         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20067         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20068         final NEDPosition nedPosition1 = new NEDPosition();
20069         assertTrue(calibrator.getNedPosition(nedPosition1));
20070         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20071         assertNull(calibrator.getMeasurements());
20072         assertFalse(calibrator.isCommonAxisUsed());
20073         assertNull(calibrator.getListener());
20074         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20075         assertFalse(calibrator.isReady());
20076         assertFalse(calibrator.isRunning());
20077         assertNull(calibrator.getEstimatedBiases());
20078         assertFalse(calibrator.getEstimatedBiases(null));
20079         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20080         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20081         assertNull(calibrator.getEstimatedBiasFx());
20082         assertNull(calibrator.getEstimatedBiasFy());
20083         assertNull(calibrator.getEstimatedBiasFz());
20084         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20085         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20086         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20087         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20088         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20089         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20090         assertNull(calibrator.getEstimatedMa());
20091         assertNull(calibrator.getEstimatedSx());
20092         assertNull(calibrator.getEstimatedSy());
20093         assertNull(calibrator.getEstimatedSz());
20094         assertNull(calibrator.getEstimatedMxy());
20095         assertNull(calibrator.getEstimatedMxz());
20096         assertNull(calibrator.getEstimatedMyx());
20097         assertNull(calibrator.getEstimatedMyz());
20098         assertNull(calibrator.getEstimatedMzx());
20099         assertNull(calibrator.getEstimatedMzy());
20100         assertNull(calibrator.getEstimatedCovariance());
20101         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20102         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20103                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20104         assertNotNull(calibrator.getGroundTruthGravityNorm());
20105         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20106         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20107         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20108                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20109         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20110         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20111         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20112     }
20113 
20114     @Test
20115     public void testConstructor158() throws WrongSizeException {
20116         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20117         final double latitude = Math.toRadians(
20118                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20119         final double longitude = Math.toRadians(
20120                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20121         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20122         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20123         final NEDVelocity nedVelocity = new NEDVelocity();
20124         final ECEFPosition ecefPosition = new ECEFPosition();
20125         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20126         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20127                 ecefPosition, ecefVelocity);
20128 
20129         final KnownPositionAccelerometerCalibrator calibrator =
20130                 new KnownPositionAccelerometerCalibrator(nedPosition, this);
20131 
20132         // check default values
20133         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20134         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20135         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20136         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20137         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20138         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20139         final Acceleration bx2 = new Acceleration(0.0,
20140                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20141         calibrator.getInitialBiasXAsAcceleration(bx2);
20142         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20143         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20144         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20145         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20146         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20147         final Acceleration by2 = new Acceleration(0.0,
20148                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20149         calibrator.getInitialBiasYAsAcceleration(by2);
20150         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20151         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20152         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20153         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20154         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20155         final Acceleration bz2 = new Acceleration(0.0,
20156                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20157         calibrator.getInitialBiasZAsAcceleration(bz2);
20158         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20159         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20160         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20161         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20162         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20163         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20164         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20165         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20166         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20167         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20168         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20169         final double[] bias1 = calibrator.getInitialBias();
20170         assertArrayEquals(bias1, new double[3], 0.0);
20171         final double[] bias2 = new double[3];
20172         calibrator.getInitialBias(bias2);
20173         assertArrayEquals(bias1, bias2, 0.0);
20174         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20175         assertEquals(b1, new Matrix(3, 1));
20176         final Matrix b2 = new Matrix(3, 1);
20177         calibrator.getInitialBiasAsMatrix(b2);
20178         assertEquals(b1, b2);
20179         final Matrix ma1 = calibrator.getInitialMa();
20180         assertEquals(ma1, new Matrix(3, 3));
20181         final Matrix ma2 = new Matrix(3, 3);
20182         calibrator.getInitialMa(ma2);
20183         assertEquals(ma1, ma2);
20184         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20185         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20186         final NEDPosition nedPosition1 = new NEDPosition();
20187         assertTrue(calibrator.getNedPosition(nedPosition1));
20188         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20189         assertNull(calibrator.getMeasurements());
20190         assertFalse(calibrator.isCommonAxisUsed());
20191         assertSame(calibrator.getListener(), this);
20192         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20193         assertFalse(calibrator.isReady());
20194         assertFalse(calibrator.isRunning());
20195         assertNull(calibrator.getEstimatedBiases());
20196         assertFalse(calibrator.getEstimatedBiases(null));
20197         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20198         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20199         assertNull(calibrator.getEstimatedBiasFx());
20200         assertNull(calibrator.getEstimatedBiasFy());
20201         assertNull(calibrator.getEstimatedBiasFz());
20202         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20203         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20204         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20205         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20206         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20207         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20208         assertNull(calibrator.getEstimatedMa());
20209         assertNull(calibrator.getEstimatedSx());
20210         assertNull(calibrator.getEstimatedSy());
20211         assertNull(calibrator.getEstimatedSz());
20212         assertNull(calibrator.getEstimatedMxy());
20213         assertNull(calibrator.getEstimatedMxz());
20214         assertNull(calibrator.getEstimatedMyx());
20215         assertNull(calibrator.getEstimatedMyz());
20216         assertNull(calibrator.getEstimatedMzx());
20217         assertNull(calibrator.getEstimatedMzy());
20218         assertNull(calibrator.getEstimatedCovariance());
20219         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20220         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20221                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20222         assertNotNull(calibrator.getGroundTruthGravityNorm());
20223         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20224         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20225         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20226                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20227         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20228         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20229         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20230     }
20231 
20232     @Test
20233     public void testConstructor159() throws WrongSizeException {
20234         final Collection<StandardDeviationBodyKinematics> measurements =
20235                 Collections.emptyList();
20236 
20237         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20238         final double latitude = Math.toRadians(
20239                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20240         final double longitude = Math.toRadians(
20241                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20242         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20243         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20244         final NEDVelocity nedVelocity = new NEDVelocity();
20245         final ECEFPosition ecefPosition = new ECEFPosition();
20246         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20247         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20248                 ecefPosition, ecefVelocity);
20249 
20250         final KnownPositionAccelerometerCalibrator calibrator =
20251                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements);
20252 
20253         // check default values
20254         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20255         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20256         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20257         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20258         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20259         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20260         final Acceleration bx2 = new Acceleration(0.0,
20261                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20262         calibrator.getInitialBiasXAsAcceleration(bx2);
20263         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20264         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20265         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20266         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20267         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20268         final Acceleration by2 = new Acceleration(0.0,
20269                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20270         calibrator.getInitialBiasYAsAcceleration(by2);
20271         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20272         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20273         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20274         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20275         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20276         final Acceleration bz2 = new Acceleration(0.0,
20277                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20278         calibrator.getInitialBiasZAsAcceleration(bz2);
20279         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20280         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20281         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20282         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20283         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20284         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20285         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20286         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20287         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20288         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20289         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20290         final double[] bias1 = calibrator.getInitialBias();
20291         assertArrayEquals(bias1, new double[3], 0.0);
20292         final double[] bias2 = new double[3];
20293         calibrator.getInitialBias(bias2);
20294         assertArrayEquals(bias1, bias2, 0.0);
20295         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20296         assertEquals(b1, new Matrix(3, 1));
20297         final Matrix b2 = new Matrix(3, 1);
20298         calibrator.getInitialBiasAsMatrix(b2);
20299         assertEquals(b1, b2);
20300         final Matrix ma1 = calibrator.getInitialMa();
20301         assertEquals(ma1, new Matrix(3, 3));
20302         final Matrix ma2 = new Matrix(3, 3);
20303         calibrator.getInitialMa(ma2);
20304         assertEquals(ma1, ma2);
20305         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20306         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20307         final NEDPosition nedPosition1 = new NEDPosition();
20308         assertTrue(calibrator.getNedPosition(nedPosition1));
20309         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20310         assertSame(calibrator.getMeasurements(), measurements);
20311         assertFalse(calibrator.isCommonAxisUsed());
20312         assertNull(calibrator.getListener());
20313         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20314         assertFalse(calibrator.isReady());
20315         assertFalse(calibrator.isRunning());
20316         assertNull(calibrator.getEstimatedBiases());
20317         assertFalse(calibrator.getEstimatedBiases(null));
20318         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20319         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20320         assertNull(calibrator.getEstimatedBiasFx());
20321         assertNull(calibrator.getEstimatedBiasFy());
20322         assertNull(calibrator.getEstimatedBiasFz());
20323         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20324         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20325         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20326         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20327         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20328         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20329         assertNull(calibrator.getEstimatedMa());
20330         assertNull(calibrator.getEstimatedSx());
20331         assertNull(calibrator.getEstimatedSy());
20332         assertNull(calibrator.getEstimatedSz());
20333         assertNull(calibrator.getEstimatedMxy());
20334         assertNull(calibrator.getEstimatedMxz());
20335         assertNull(calibrator.getEstimatedMyx());
20336         assertNull(calibrator.getEstimatedMyz());
20337         assertNull(calibrator.getEstimatedMzx());
20338         assertNull(calibrator.getEstimatedMzy());
20339         assertNull(calibrator.getEstimatedCovariance());
20340         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20341         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20342                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20343         assertNotNull(calibrator.getGroundTruthGravityNorm());
20344         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20345         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20346         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20347                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20348         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20349         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20350         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20351     }
20352 
20353     @Test
20354     public void testConstructor160() throws WrongSizeException {
20355         final Collection<StandardDeviationBodyKinematics> measurements =
20356                 Collections.emptyList();
20357 
20358         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20359         final double latitude = Math.toRadians(
20360                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20361         final double longitude = Math.toRadians(
20362                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20363         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20364         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20365         final NEDVelocity nedVelocity = new NEDVelocity();
20366         final ECEFPosition ecefPosition = new ECEFPosition();
20367         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20368         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20369                 ecefPosition, ecefVelocity);
20370 
20371         final KnownPositionAccelerometerCalibrator calibrator =
20372                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20373                         this);
20374 
20375         // check default values
20376         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20377         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20378         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20379         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20380         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20381         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20382         final Acceleration bx2 = new Acceleration(0.0,
20383                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20384         calibrator.getInitialBiasXAsAcceleration(bx2);
20385         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20386         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20387         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20388         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20389         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20390         final Acceleration by2 = new Acceleration(0.0,
20391                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20392         calibrator.getInitialBiasYAsAcceleration(by2);
20393         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20394         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20395         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20396         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20397         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20398         final Acceleration bz2 = new Acceleration(0.0,
20399                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20400         calibrator.getInitialBiasZAsAcceleration(bz2);
20401         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20402         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20403         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20404         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20405         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20406         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20407         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20408         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20409         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20410         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20411         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20412         final double[] bias1 = calibrator.getInitialBias();
20413         assertArrayEquals(bias1, new double[3], 0.0);
20414         final double[] bias2 = new double[3];
20415         calibrator.getInitialBias(bias2);
20416         assertArrayEquals(bias1, bias2, 0.0);
20417         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20418         assertEquals(b1, new Matrix(3, 1));
20419         final Matrix b2 = new Matrix(3, 1);
20420         calibrator.getInitialBiasAsMatrix(b2);
20421         assertEquals(b1, b2);
20422         final Matrix ma1 = calibrator.getInitialMa();
20423         assertEquals(ma1, new Matrix(3, 3));
20424         final Matrix ma2 = new Matrix(3, 3);
20425         calibrator.getInitialMa(ma2);
20426         assertEquals(ma1, ma2);
20427         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20428         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20429         final NEDPosition nedPosition1 = new NEDPosition();
20430         assertTrue(calibrator.getNedPosition(nedPosition1));
20431         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20432         assertSame(calibrator.getMeasurements(), measurements);
20433         assertFalse(calibrator.isCommonAxisUsed());
20434         assertSame(calibrator.getListener(), this);
20435         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20436         assertFalse(calibrator.isReady());
20437         assertFalse(calibrator.isRunning());
20438         assertNull(calibrator.getEstimatedBiases());
20439         assertFalse(calibrator.getEstimatedBiases(null));
20440         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20441         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20442         assertNull(calibrator.getEstimatedBiasFx());
20443         assertNull(calibrator.getEstimatedBiasFy());
20444         assertNull(calibrator.getEstimatedBiasFz());
20445         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20446         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20447         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20448         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20449         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20450         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20451         assertNull(calibrator.getEstimatedMa());
20452         assertNull(calibrator.getEstimatedSx());
20453         assertNull(calibrator.getEstimatedSy());
20454         assertNull(calibrator.getEstimatedSz());
20455         assertNull(calibrator.getEstimatedMxy());
20456         assertNull(calibrator.getEstimatedMxz());
20457         assertNull(calibrator.getEstimatedMyx());
20458         assertNull(calibrator.getEstimatedMyz());
20459         assertNull(calibrator.getEstimatedMzx());
20460         assertNull(calibrator.getEstimatedMzy());
20461         assertNull(calibrator.getEstimatedCovariance());
20462         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20463         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20464                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20465         assertNotNull(calibrator.getGroundTruthGravityNorm());
20466         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20467         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20468         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20469                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20470         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20471         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20472         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20473     }
20474 
20475     @Test
20476     public void testConstructor161() throws WrongSizeException {
20477         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20478         final double latitude = Math.toRadians(
20479                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20480         final double longitude = Math.toRadians(
20481                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20482         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20483         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20484         final NEDVelocity nedVelocity = new NEDVelocity();
20485         final ECEFPosition ecefPosition = new ECEFPosition();
20486         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20487         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20488                 ecefPosition, ecefVelocity);
20489 
20490         final KnownPositionAccelerometerCalibrator calibrator =
20491                 new KnownPositionAccelerometerCalibrator(nedPosition,
20492                         true);
20493 
20494         // check default values
20495         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20496         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20497         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20498         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20499         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20500         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20501         final Acceleration bx2 = new Acceleration(0.0,
20502                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20503         calibrator.getInitialBiasXAsAcceleration(bx2);
20504         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20505         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20506         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20507         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20508         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20509         final Acceleration by2 = new Acceleration(0.0,
20510                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20511         calibrator.getInitialBiasYAsAcceleration(by2);
20512         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20513         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20514         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20515         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20516         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20517         final Acceleration bz2 = new Acceleration(0.0,
20518                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20519         calibrator.getInitialBiasZAsAcceleration(bz2);
20520         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20521         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20522         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20523         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20524         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20525         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20526         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20527         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20528         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20529         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20530         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20531         final double[] bias1 = calibrator.getInitialBias();
20532         assertArrayEquals(bias1, new double[3], 0.0);
20533         final double[] bias2 = new double[3];
20534         calibrator.getInitialBias(bias2);
20535         assertArrayEquals(bias1, bias2, 0.0);
20536         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20537         assertEquals(b1, new Matrix(3, 1));
20538         final Matrix b2 = new Matrix(3, 1);
20539         calibrator.getInitialBiasAsMatrix(b2);
20540         assertEquals(b1, b2);
20541         final Matrix ma1 = calibrator.getInitialMa();
20542         assertEquals(ma1, new Matrix(3, 3));
20543         final Matrix ma2 = new Matrix(3, 3);
20544         calibrator.getInitialMa(ma2);
20545         assertEquals(ma1, ma2);
20546         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20547         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20548         final NEDPosition nedPosition1 = new NEDPosition();
20549         assertTrue(calibrator.getNedPosition(nedPosition1));
20550         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20551         assertNull(calibrator.getMeasurements());
20552         assertTrue(calibrator.isCommonAxisUsed());
20553         assertNull(calibrator.getListener());
20554         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20555         assertFalse(calibrator.isReady());
20556         assertFalse(calibrator.isRunning());
20557         assertNull(calibrator.getEstimatedBiases());
20558         assertFalse(calibrator.getEstimatedBiases(null));
20559         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20560         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20561         assertNull(calibrator.getEstimatedBiasFx());
20562         assertNull(calibrator.getEstimatedBiasFy());
20563         assertNull(calibrator.getEstimatedBiasFz());
20564         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20565         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20566         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20567         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20568         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20569         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20570         assertNull(calibrator.getEstimatedMa());
20571         assertNull(calibrator.getEstimatedSx());
20572         assertNull(calibrator.getEstimatedSy());
20573         assertNull(calibrator.getEstimatedSz());
20574         assertNull(calibrator.getEstimatedMxy());
20575         assertNull(calibrator.getEstimatedMxz());
20576         assertNull(calibrator.getEstimatedMyx());
20577         assertNull(calibrator.getEstimatedMyz());
20578         assertNull(calibrator.getEstimatedMzx());
20579         assertNull(calibrator.getEstimatedMzy());
20580         assertNull(calibrator.getEstimatedCovariance());
20581         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20582         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20583                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20584         assertNotNull(calibrator.getGroundTruthGravityNorm());
20585         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20586         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20587         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20588                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20589         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20590         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20591         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20592     }
20593 
20594     @Test
20595     public void testConstructor162() throws WrongSizeException {
20596         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20597         final double latitude = Math.toRadians(
20598                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20599         final double longitude = Math.toRadians(
20600                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20601         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20602         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20603         final NEDVelocity nedVelocity = new NEDVelocity();
20604         final ECEFPosition ecefPosition = new ECEFPosition();
20605         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20606         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20607                 ecefPosition, ecefVelocity);
20608 
20609         final KnownPositionAccelerometerCalibrator calibrator =
20610                 new KnownPositionAccelerometerCalibrator(nedPosition,
20611                         true, this);
20612 
20613         // check default values
20614         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20615         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20616         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20617         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20618         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20619         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20620         final Acceleration bx2 = new Acceleration(0.0,
20621                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20622         calibrator.getInitialBiasXAsAcceleration(bx2);
20623         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20624         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20625         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20626         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20627         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20628         final Acceleration by2 = new Acceleration(0.0,
20629                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20630         calibrator.getInitialBiasYAsAcceleration(by2);
20631         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20632         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20633         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20634         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20635         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20636         final Acceleration bz2 = new Acceleration(0.0,
20637                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20638         calibrator.getInitialBiasZAsAcceleration(bz2);
20639         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20640         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20641         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20642         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20643         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20644         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20645         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20646         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20647         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20648         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20649         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20650         final double[] bias1 = calibrator.getInitialBias();
20651         assertArrayEquals(bias1, new double[3], 0.0);
20652         final double[] bias2 = new double[3];
20653         calibrator.getInitialBias(bias2);
20654         assertArrayEquals(bias1, bias2, 0.0);
20655         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20656         assertEquals(b1, new Matrix(3, 1));
20657         final Matrix b2 = new Matrix(3, 1);
20658         calibrator.getInitialBiasAsMatrix(b2);
20659         assertEquals(b1, b2);
20660         final Matrix ma1 = calibrator.getInitialMa();
20661         assertEquals(ma1, new Matrix(3, 3));
20662         final Matrix ma2 = new Matrix(3, 3);
20663         calibrator.getInitialMa(ma2);
20664         assertEquals(ma1, ma2);
20665         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20666         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20667         final NEDPosition nedPosition1 = new NEDPosition();
20668         assertTrue(calibrator.getNedPosition(nedPosition1));
20669         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20670         assertNull(calibrator.getMeasurements());
20671         assertTrue(calibrator.isCommonAxisUsed());
20672         assertSame(calibrator.getListener(), this);
20673         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20674         assertFalse(calibrator.isReady());
20675         assertFalse(calibrator.isRunning());
20676         assertNull(calibrator.getEstimatedBiases());
20677         assertFalse(calibrator.getEstimatedBiases(null));
20678         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20679         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20680         assertNull(calibrator.getEstimatedBiasFx());
20681         assertNull(calibrator.getEstimatedBiasFy());
20682         assertNull(calibrator.getEstimatedBiasFz());
20683         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20684         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20685         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20686         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20687         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20688         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20689         assertNull(calibrator.getEstimatedMa());
20690         assertNull(calibrator.getEstimatedSx());
20691         assertNull(calibrator.getEstimatedSy());
20692         assertNull(calibrator.getEstimatedSz());
20693         assertNull(calibrator.getEstimatedMxy());
20694         assertNull(calibrator.getEstimatedMxz());
20695         assertNull(calibrator.getEstimatedMyx());
20696         assertNull(calibrator.getEstimatedMyz());
20697         assertNull(calibrator.getEstimatedMzx());
20698         assertNull(calibrator.getEstimatedMzy());
20699         assertNull(calibrator.getEstimatedCovariance());
20700         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20701         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20702                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20703         assertNotNull(calibrator.getGroundTruthGravityNorm());
20704         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20705         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20706         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20707                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20708         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20709         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20710         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20711     }
20712 
20713     @Test
20714     public void testConstructor163() throws WrongSizeException {
20715         final Collection<StandardDeviationBodyKinematics> measurements =
20716                 Collections.emptyList();
20717         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20718         final double latitude = Math.toRadians(
20719                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20720         final double longitude = Math.toRadians(
20721                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20722         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20723         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20724         final NEDVelocity nedVelocity = new NEDVelocity();
20725         final ECEFPosition ecefPosition = new ECEFPosition();
20726         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20727         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20728                 ecefPosition, ecefVelocity);
20729 
20730         final KnownPositionAccelerometerCalibrator calibrator =
20731                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20732                         true);
20733 
20734         // check default values
20735         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20736         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20737         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20738         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20739         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20740         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20741         final Acceleration bx2 = new Acceleration(0.0,
20742                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20743         calibrator.getInitialBiasXAsAcceleration(bx2);
20744         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20745         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20746         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20747         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20748         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20749         final Acceleration by2 = new Acceleration(0.0,
20750                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20751         calibrator.getInitialBiasYAsAcceleration(by2);
20752         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20753         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20754         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20755         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20756         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20757         final Acceleration bz2 = new Acceleration(0.0,
20758                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20759         calibrator.getInitialBiasZAsAcceleration(bz2);
20760         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20761         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20762         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20763         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20764         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20765         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20766         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20767         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20768         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20769         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20770         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20771         final double[] bias1 = calibrator.getInitialBias();
20772         assertArrayEquals(bias1, new double[3], 0.0);
20773         final double[] bias2 = new double[3];
20774         calibrator.getInitialBias(bias2);
20775         assertArrayEquals(bias1, bias2, 0.0);
20776         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20777         assertEquals(b1, new Matrix(3, 1));
20778         final Matrix b2 = new Matrix(3, 1);
20779         calibrator.getInitialBiasAsMatrix(b2);
20780         assertEquals(b1, b2);
20781         final Matrix ma1 = calibrator.getInitialMa();
20782         assertEquals(ma1, new Matrix(3, 3));
20783         final Matrix ma2 = new Matrix(3, 3);
20784         calibrator.getInitialMa(ma2);
20785         assertEquals(ma1, ma2);
20786         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20787         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20788         final NEDPosition nedPosition1 = new NEDPosition();
20789         assertTrue(calibrator.getNedPosition(nedPosition1));
20790         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20791         assertSame(calibrator.getMeasurements(), measurements);
20792         assertTrue(calibrator.isCommonAxisUsed());
20793         assertNull(calibrator.getListener());
20794         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20795         assertFalse(calibrator.isReady());
20796         assertFalse(calibrator.isRunning());
20797         assertNull(calibrator.getEstimatedBiases());
20798         assertFalse(calibrator.getEstimatedBiases(null));
20799         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20800         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20801         assertNull(calibrator.getEstimatedBiasFx());
20802         assertNull(calibrator.getEstimatedBiasFy());
20803         assertNull(calibrator.getEstimatedBiasFz());
20804         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20805         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20806         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20807         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20808         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20809         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20810         assertNull(calibrator.getEstimatedMa());
20811         assertNull(calibrator.getEstimatedSx());
20812         assertNull(calibrator.getEstimatedSy());
20813         assertNull(calibrator.getEstimatedSz());
20814         assertNull(calibrator.getEstimatedMxy());
20815         assertNull(calibrator.getEstimatedMxz());
20816         assertNull(calibrator.getEstimatedMyx());
20817         assertNull(calibrator.getEstimatedMyz());
20818         assertNull(calibrator.getEstimatedMzx());
20819         assertNull(calibrator.getEstimatedMzy());
20820         assertNull(calibrator.getEstimatedCovariance());
20821         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20822         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20823                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20824         assertNotNull(calibrator.getGroundTruthGravityNorm());
20825         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20826         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20827         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20828                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20829         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20830         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20831         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20832     }
20833 
20834     @Test
20835     public void testConstructor164() throws WrongSizeException {
20836         final Collection<StandardDeviationBodyKinematics> measurements =
20837                 Collections.emptyList();
20838         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20839         final double latitude = Math.toRadians(
20840                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20841         final double longitude = Math.toRadians(
20842                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20843         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20844         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20845         final NEDVelocity nedVelocity = new NEDVelocity();
20846         final ECEFPosition ecefPosition = new ECEFPosition();
20847         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20848         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20849                 ecefPosition, ecefVelocity);
20850 
20851         final KnownPositionAccelerometerCalibrator calibrator =
20852                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20853                         true, this);
20854 
20855         // check default values
20856         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20857         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20858         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20859         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20860         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20861         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20862         final Acceleration bx2 = new Acceleration(0.0,
20863                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20864         calibrator.getInitialBiasXAsAcceleration(bx2);
20865         assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20866         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20867         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20868         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20869         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20870         final Acceleration by2 = new Acceleration(0.0,
20871                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20872         calibrator.getInitialBiasYAsAcceleration(by2);
20873         assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20874         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20875         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20876         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20877         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20878         final Acceleration bz2 = new Acceleration(0.0,
20879                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20880         calibrator.getInitialBiasZAsAcceleration(bz2);
20881         assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20882         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20883         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20884         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20885         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20886         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20887         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20888         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20889         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20890         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20891         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20892         final double[] bias1 = calibrator.getInitialBias();
20893         assertArrayEquals(bias1, new double[3], 0.0);
20894         final double[] bias2 = new double[3];
20895         calibrator.getInitialBias(bias2);
20896         assertArrayEquals(bias1, bias2, 0.0);
20897         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20898         assertEquals(b1, new Matrix(3, 1));
20899         final Matrix b2 = new Matrix(3, 1);
20900         calibrator.getInitialBiasAsMatrix(b2);
20901         assertEquals(b1, b2);
20902         final Matrix ma1 = calibrator.getInitialMa();
20903         assertEquals(ma1, new Matrix(3, 3));
20904         final Matrix ma2 = new Matrix(3, 3);
20905         calibrator.getInitialMa(ma2);
20906         assertEquals(ma1, ma2);
20907         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20908         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20909         final NEDPosition nedPosition1 = new NEDPosition();
20910         assertTrue(calibrator.getNedPosition(nedPosition1));
20911         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20912         assertSame(calibrator.getMeasurements(), measurements);
20913         assertTrue(calibrator.isCommonAxisUsed());
20914         assertSame(calibrator.getListener(), this);
20915         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20916         assertFalse(calibrator.isReady());
20917         assertFalse(calibrator.isRunning());
20918         assertNull(calibrator.getEstimatedBiases());
20919         assertFalse(calibrator.getEstimatedBiases(null));
20920         assertNull(calibrator.getEstimatedBiasesAsMatrix());
20921         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20922         assertNull(calibrator.getEstimatedBiasFx());
20923         assertNull(calibrator.getEstimatedBiasFy());
20924         assertNull(calibrator.getEstimatedBiasFz());
20925         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20926         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20927         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20928         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20929         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20930         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20931         assertNull(calibrator.getEstimatedMa());
20932         assertNull(calibrator.getEstimatedSx());
20933         assertNull(calibrator.getEstimatedSy());
20934         assertNull(calibrator.getEstimatedSz());
20935         assertNull(calibrator.getEstimatedMxy());
20936         assertNull(calibrator.getEstimatedMxz());
20937         assertNull(calibrator.getEstimatedMyx());
20938         assertNull(calibrator.getEstimatedMyz());
20939         assertNull(calibrator.getEstimatedMzx());
20940         assertNull(calibrator.getEstimatedMzy());
20941         assertNull(calibrator.getEstimatedCovariance());
20942         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20943         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20944                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20945         assertNotNull(calibrator.getGroundTruthGravityNorm());
20946         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20947         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20948         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20949                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20950         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20951         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20952         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20953     }
20954 
20955     @Test
20956     public void testConstructor165() throws WrongSizeException {
20957         final Matrix ba = generateBa();
20958         final double biasX = ba.getElementAtIndex(0);
20959         final double biasY = ba.getElementAtIndex(1);
20960         final double biasZ = ba.getElementAtIndex(2);
20961 
20962         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20963         final double latitude = Math.toRadians(
20964                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20965         final double longitude = Math.toRadians(
20966                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20967         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20968         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20969         final NEDVelocity nedVelocity = new NEDVelocity();
20970         final ECEFPosition ecefPosition = new ECEFPosition();
20971         final ECEFVelocity ecefVelocity = new ECEFVelocity();
20972         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20973                 ecefPosition, ecefVelocity);
20974 
20975         final KnownPositionAccelerometerCalibrator calibrator =
20976                 new KnownPositionAccelerometerCalibrator(nedPosition,
20977                         biasX, biasY, biasZ);
20978 
20979         // check default values
20980         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20981         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20982         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20983         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20984         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
20985         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20986         final Acceleration bx2 = new Acceleration(0.0,
20987                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20988         calibrator.getInitialBiasXAsAcceleration(bx2);
20989         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
20990         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20991         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20992         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
20993         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20994         final Acceleration by2 = new Acceleration(0.0,
20995                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20996         calibrator.getInitialBiasYAsAcceleration(by2);
20997         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
20998         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20999         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21000         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21001         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21002         final Acceleration bz2 = new Acceleration(0.0,
21003                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21004         calibrator.getInitialBiasZAsAcceleration(bz2);
21005         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21006         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21007         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21008         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21009         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21010         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21011         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21012         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21013         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21014         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21015         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21016         final double[] bias1 = calibrator.getInitialBias();
21017         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21018         final double[] bias2 = new double[3];
21019         calibrator.getInitialBias(bias2);
21020         assertArrayEquals(bias1, bias2, 0.0);
21021         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21022         assertEquals(b1, ba);
21023         final Matrix b2 = new Matrix(3, 1);
21024         calibrator.getInitialBiasAsMatrix(b2);
21025         assertEquals(b1, b2);
21026         final Matrix ma1 = calibrator.getInitialMa();
21027         assertEquals(ma1, new Matrix(3, 3));
21028         final Matrix ma2 = new Matrix(3, 3);
21029         calibrator.getInitialMa(ma2);
21030         assertEquals(ma1, ma2);
21031         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21032         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21033         final NEDPosition nedPosition1 = new NEDPosition();
21034         assertTrue(calibrator.getNedPosition(nedPosition1));
21035         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21036         assertNull(calibrator.getMeasurements());
21037         assertFalse(calibrator.isCommonAxisUsed());
21038         assertNull(calibrator.getListener());
21039         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21040         assertFalse(calibrator.isReady());
21041         assertFalse(calibrator.isRunning());
21042         assertNull(calibrator.getEstimatedBiases());
21043         assertFalse(calibrator.getEstimatedBiases(null));
21044         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21045         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21046         assertNull(calibrator.getEstimatedBiasFx());
21047         assertNull(calibrator.getEstimatedBiasFy());
21048         assertNull(calibrator.getEstimatedBiasFz());
21049         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21050         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21051         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21052         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21053         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21054         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21055         assertNull(calibrator.getEstimatedMa());
21056         assertNull(calibrator.getEstimatedSx());
21057         assertNull(calibrator.getEstimatedSy());
21058         assertNull(calibrator.getEstimatedSz());
21059         assertNull(calibrator.getEstimatedMxy());
21060         assertNull(calibrator.getEstimatedMxz());
21061         assertNull(calibrator.getEstimatedMyx());
21062         assertNull(calibrator.getEstimatedMyz());
21063         assertNull(calibrator.getEstimatedMzx());
21064         assertNull(calibrator.getEstimatedMzy());
21065         assertNull(calibrator.getEstimatedCovariance());
21066         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21067         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21068                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21069         assertNotNull(calibrator.getGroundTruthGravityNorm());
21070         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21071         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21072         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21073                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21074         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21075         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21076         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21077     }
21078 
21079     @Test
21080     public void testConstructor166() throws WrongSizeException {
21081         final Matrix ba = generateBa();
21082         final double biasX = ba.getElementAtIndex(0);
21083         final double biasY = ba.getElementAtIndex(1);
21084         final double biasZ = ba.getElementAtIndex(2);
21085 
21086         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21087         final double latitude = Math.toRadians(
21088                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21089         final double longitude = Math.toRadians(
21090                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21091         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21092         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21093         final NEDVelocity nedVelocity = new NEDVelocity();
21094         final ECEFPosition ecefPosition = new ECEFPosition();
21095         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21096         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21097                 ecefPosition, ecefVelocity);
21098 
21099         final KnownPositionAccelerometerCalibrator calibrator =
21100                 new KnownPositionAccelerometerCalibrator(nedPosition,
21101                         biasX, biasY, biasZ, this);
21102 
21103         // check default values
21104         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21105         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21106         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21107         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21108         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21109         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21110         final Acceleration bx2 = new Acceleration(0.0,
21111                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21112         calibrator.getInitialBiasXAsAcceleration(bx2);
21113         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21114         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21115         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21116         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21117         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21118         final Acceleration by2 = new Acceleration(0.0,
21119                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21120         calibrator.getInitialBiasYAsAcceleration(by2);
21121         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21122         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21123         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21124         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21125         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21126         final Acceleration bz2 = new Acceleration(0.0,
21127                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21128         calibrator.getInitialBiasZAsAcceleration(bz2);
21129         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21130         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21131         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21132         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21133         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21134         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21135         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21136         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21137         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21138         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21139         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21140         final double[] bias1 = calibrator.getInitialBias();
21141         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21142         final double[] bias2 = new double[3];
21143         calibrator.getInitialBias(bias2);
21144         assertArrayEquals(bias1, bias2, 0.0);
21145         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21146         assertEquals(b1, ba);
21147         final Matrix b2 = new Matrix(3, 1);
21148         calibrator.getInitialBiasAsMatrix(b2);
21149         assertEquals(b1, b2);
21150         final Matrix ma1 = calibrator.getInitialMa();
21151         assertEquals(ma1, new Matrix(3, 3));
21152         final Matrix ma2 = new Matrix(3, 3);
21153         calibrator.getInitialMa(ma2);
21154         assertEquals(ma1, ma2);
21155         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21156         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21157         final NEDPosition nedPosition1 = new NEDPosition();
21158         assertTrue(calibrator.getNedPosition(nedPosition1));
21159         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21160         assertNull(calibrator.getMeasurements());
21161         assertFalse(calibrator.isCommonAxisUsed());
21162         assertSame(calibrator.getListener(), this);
21163         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21164         assertFalse(calibrator.isReady());
21165         assertFalse(calibrator.isRunning());
21166         assertNull(calibrator.getEstimatedBiases());
21167         assertFalse(calibrator.getEstimatedBiases(null));
21168         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21169         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21170         assertNull(calibrator.getEstimatedBiasFx());
21171         assertNull(calibrator.getEstimatedBiasFy());
21172         assertNull(calibrator.getEstimatedBiasFz());
21173         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21174         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21175         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21176         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21177         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21178         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21179         assertNull(calibrator.getEstimatedMa());
21180         assertNull(calibrator.getEstimatedSx());
21181         assertNull(calibrator.getEstimatedSy());
21182         assertNull(calibrator.getEstimatedSz());
21183         assertNull(calibrator.getEstimatedMxy());
21184         assertNull(calibrator.getEstimatedMxz());
21185         assertNull(calibrator.getEstimatedMyx());
21186         assertNull(calibrator.getEstimatedMyz());
21187         assertNull(calibrator.getEstimatedMzx());
21188         assertNull(calibrator.getEstimatedMzy());
21189         assertNull(calibrator.getEstimatedCovariance());
21190         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21191         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21192                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21193         assertNotNull(calibrator.getGroundTruthGravityNorm());
21194         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21195         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21196         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21197                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21198         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21199         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21200         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21201     }
21202 
21203     @Test
21204     public void testConstructor167() throws WrongSizeException {
21205         final Collection<StandardDeviationBodyKinematics> measurements =
21206                 Collections.emptyList();
21207 
21208         final Matrix ba = generateBa();
21209         final double biasX = ba.getElementAtIndex(0);
21210         final double biasY = ba.getElementAtIndex(1);
21211         final double biasZ = ba.getElementAtIndex(2);
21212 
21213         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21214         final double latitude = Math.toRadians(
21215                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21216         final double longitude = Math.toRadians(
21217                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21218         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21219         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21220         final NEDVelocity nedVelocity = new NEDVelocity();
21221         final ECEFPosition ecefPosition = new ECEFPosition();
21222         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21223         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21224                 ecefPosition, ecefVelocity);
21225 
21226         final KnownPositionAccelerometerCalibrator calibrator =
21227                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21228                         biasX, biasY, biasZ);
21229 
21230         // check default values
21231         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21232         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21233         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21234         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21235         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21236         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21237         final Acceleration bx2 = new Acceleration(0.0,
21238                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21239         calibrator.getInitialBiasXAsAcceleration(bx2);
21240         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21241         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21242         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21243         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21244         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21245         final Acceleration by2 = new Acceleration(0.0,
21246                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21247         calibrator.getInitialBiasYAsAcceleration(by2);
21248         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21249         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21250         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21251         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21252         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21253         final Acceleration bz2 = new Acceleration(0.0,
21254                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21255         calibrator.getInitialBiasZAsAcceleration(bz2);
21256         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21257         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21258         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21259         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21260         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21261         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21262         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21263         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21264         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21265         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21266         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21267         final double[] bias1 = calibrator.getInitialBias();
21268         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21269         final double[] bias2 = new double[3];
21270         calibrator.getInitialBias(bias2);
21271         assertArrayEquals(bias1, bias2, 0.0);
21272         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21273         assertEquals(b1, ba);
21274         final Matrix b2 = new Matrix(3, 1);
21275         calibrator.getInitialBiasAsMatrix(b2);
21276         assertEquals(b1, b2);
21277         final Matrix ma1 = calibrator.getInitialMa();
21278         assertEquals(ma1, new Matrix(3, 3));
21279         final Matrix ma2 = new Matrix(3, 3);
21280         calibrator.getInitialMa(ma2);
21281         assertEquals(ma1, ma2);
21282         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21283         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21284         final NEDPosition nedPosition1 = new NEDPosition();
21285         assertTrue(calibrator.getNedPosition(nedPosition1));
21286         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21287         assertSame(calibrator.getMeasurements(), measurements);
21288         assertFalse(calibrator.isCommonAxisUsed());
21289         assertNull(calibrator.getListener());
21290         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21291         assertFalse(calibrator.isReady());
21292         assertFalse(calibrator.isRunning());
21293         assertNull(calibrator.getEstimatedBiases());
21294         assertFalse(calibrator.getEstimatedBiases(null));
21295         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21296         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21297         assertNull(calibrator.getEstimatedBiasFx());
21298         assertNull(calibrator.getEstimatedBiasFy());
21299         assertNull(calibrator.getEstimatedBiasFz());
21300         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21301         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21302         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21303         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21304         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21305         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21306         assertNull(calibrator.getEstimatedMa());
21307         assertNull(calibrator.getEstimatedSx());
21308         assertNull(calibrator.getEstimatedSy());
21309         assertNull(calibrator.getEstimatedSz());
21310         assertNull(calibrator.getEstimatedMxy());
21311         assertNull(calibrator.getEstimatedMxz());
21312         assertNull(calibrator.getEstimatedMyx());
21313         assertNull(calibrator.getEstimatedMyz());
21314         assertNull(calibrator.getEstimatedMzx());
21315         assertNull(calibrator.getEstimatedMzy());
21316         assertNull(calibrator.getEstimatedCovariance());
21317         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21318         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21319                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21320         assertNotNull(calibrator.getGroundTruthGravityNorm());
21321         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21322         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21323         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21324                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21325         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21326         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21327         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21328     }
21329 
21330     @Test
21331     public void testConstructor168() throws WrongSizeException {
21332         final Collection<StandardDeviationBodyKinematics> measurements =
21333                 Collections.emptyList();
21334 
21335         final Matrix ba = generateBa();
21336         final double biasX = ba.getElementAtIndex(0);
21337         final double biasY = ba.getElementAtIndex(1);
21338         final double biasZ = ba.getElementAtIndex(2);
21339 
21340         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21341         final double latitude = Math.toRadians(
21342                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21343         final double longitude = Math.toRadians(
21344                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21345         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21346         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21347         final NEDVelocity nedVelocity = new NEDVelocity();
21348         final ECEFPosition ecefPosition = new ECEFPosition();
21349         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21350         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21351                 ecefPosition, ecefVelocity);
21352 
21353         final KnownPositionAccelerometerCalibrator calibrator =
21354                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21355                         biasX, biasY, biasZ, this);
21356 
21357         // check default values
21358         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21359         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21360         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21361         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21362         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21363         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21364         final Acceleration bx2 = new Acceleration(0.0,
21365                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21366         calibrator.getInitialBiasXAsAcceleration(bx2);
21367         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21368         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21369         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21370         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21371         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21372         final Acceleration by2 = new Acceleration(0.0,
21373                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21374         calibrator.getInitialBiasYAsAcceleration(by2);
21375         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21376         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21377         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21378         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21379         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21380         final Acceleration bz2 = new Acceleration(0.0,
21381                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21382         calibrator.getInitialBiasZAsAcceleration(bz2);
21383         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21384         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21385         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21386         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21387         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21388         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21389         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21390         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21391         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21392         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21393         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21394         final double[] bias1 = calibrator.getInitialBias();
21395         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21396         final double[] bias2 = new double[3];
21397         calibrator.getInitialBias(bias2);
21398         assertArrayEquals(bias1, bias2, 0.0);
21399         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21400         assertEquals(b1, ba);
21401         final Matrix b2 = new Matrix(3, 1);
21402         calibrator.getInitialBiasAsMatrix(b2);
21403         assertEquals(b1, b2);
21404         final Matrix ma1 = calibrator.getInitialMa();
21405         assertEquals(ma1, new Matrix(3, 3));
21406         final Matrix ma2 = new Matrix(3, 3);
21407         calibrator.getInitialMa(ma2);
21408         assertEquals(ma1, ma2);
21409         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21410         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21411         final NEDPosition nedPosition1 = new NEDPosition();
21412         assertTrue(calibrator.getNedPosition(nedPosition1));
21413         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21414         assertSame(calibrator.getMeasurements(), measurements);
21415         assertFalse(calibrator.isCommonAxisUsed());
21416         assertSame(calibrator.getListener(), this);
21417         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21418         assertFalse(calibrator.isReady());
21419         assertFalse(calibrator.isRunning());
21420         assertNull(calibrator.getEstimatedBiases());
21421         assertFalse(calibrator.getEstimatedBiases(null));
21422         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21423         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21424         assertNull(calibrator.getEstimatedBiasFx());
21425         assertNull(calibrator.getEstimatedBiasFy());
21426         assertNull(calibrator.getEstimatedBiasFz());
21427         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21428         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21429         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21430         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21431         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21432         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21433         assertNull(calibrator.getEstimatedMa());
21434         assertNull(calibrator.getEstimatedSx());
21435         assertNull(calibrator.getEstimatedSy());
21436         assertNull(calibrator.getEstimatedSz());
21437         assertNull(calibrator.getEstimatedMxy());
21438         assertNull(calibrator.getEstimatedMxz());
21439         assertNull(calibrator.getEstimatedMyx());
21440         assertNull(calibrator.getEstimatedMyz());
21441         assertNull(calibrator.getEstimatedMzx());
21442         assertNull(calibrator.getEstimatedMzy());
21443         assertNull(calibrator.getEstimatedCovariance());
21444         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21445         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21446                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21447         assertNotNull(calibrator.getGroundTruthGravityNorm());
21448         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21449         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21450         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21451                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21452         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21453         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21454         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21455     }
21456 
21457     @Test
21458     public void testConstructor169() throws WrongSizeException {
21459         final Matrix ba = generateBa();
21460         final double biasX = ba.getElementAtIndex(0);
21461         final double biasY = ba.getElementAtIndex(1);
21462         final double biasZ = ba.getElementAtIndex(2);
21463 
21464         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21465         final double latitude = Math.toRadians(
21466                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21467         final double longitude = Math.toRadians(
21468                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21469         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21470         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21471         final NEDVelocity nedVelocity = new NEDVelocity();
21472         final ECEFPosition ecefPosition = new ECEFPosition();
21473         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21474         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21475                 ecefPosition, ecefVelocity);
21476 
21477         final KnownPositionAccelerometerCalibrator calibrator =
21478                 new KnownPositionAccelerometerCalibrator(nedPosition,
21479                         true, biasX, biasY, biasZ);
21480 
21481         // check default values
21482         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21483         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21484         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21485         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21486         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21487         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21488         final Acceleration bx2 = new Acceleration(0.0,
21489                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21490         calibrator.getInitialBiasXAsAcceleration(bx2);
21491         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21492         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21493         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21494         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21495         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21496         final Acceleration by2 = new Acceleration(0.0,
21497                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21498         calibrator.getInitialBiasYAsAcceleration(by2);
21499         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21500         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21501         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21502         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21503         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21504         final Acceleration bz2 = new Acceleration(0.0,
21505                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21506         calibrator.getInitialBiasZAsAcceleration(bz2);
21507         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21508         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21509         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21510         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21511         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21512         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21513         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21514         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21515         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21516         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21517         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21518         final double[] bias1 = calibrator.getInitialBias();
21519         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21520         final double[] bias2 = new double[3];
21521         calibrator.getInitialBias(bias2);
21522         assertArrayEquals(bias1, bias2, 0.0);
21523         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21524         assertEquals(b1, ba);
21525         final Matrix b2 = new Matrix(3, 1);
21526         calibrator.getInitialBiasAsMatrix(b2);
21527         assertEquals(b1, b2);
21528         final Matrix ma1 = calibrator.getInitialMa();
21529         assertEquals(ma1, new Matrix(3, 3));
21530         final Matrix ma2 = new Matrix(3, 3);
21531         calibrator.getInitialMa(ma2);
21532         assertEquals(ma1, ma2);
21533         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21534         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21535         final NEDPosition nedPosition1 = new NEDPosition();
21536         assertTrue(calibrator.getNedPosition(nedPosition1));
21537         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21538         assertNull(calibrator.getMeasurements());
21539         assertTrue(calibrator.isCommonAxisUsed());
21540         assertNull(calibrator.getListener());
21541         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21542         assertFalse(calibrator.isReady());
21543         assertFalse(calibrator.isRunning());
21544         assertNull(calibrator.getEstimatedBiases());
21545         assertFalse(calibrator.getEstimatedBiases(null));
21546         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21547         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21548         assertNull(calibrator.getEstimatedBiasFx());
21549         assertNull(calibrator.getEstimatedBiasFy());
21550         assertNull(calibrator.getEstimatedBiasFz());
21551         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21552         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21553         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21554         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21555         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21556         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21557         assertNull(calibrator.getEstimatedMa());
21558         assertNull(calibrator.getEstimatedSx());
21559         assertNull(calibrator.getEstimatedSy());
21560         assertNull(calibrator.getEstimatedSz());
21561         assertNull(calibrator.getEstimatedMxy());
21562         assertNull(calibrator.getEstimatedMxz());
21563         assertNull(calibrator.getEstimatedMyx());
21564         assertNull(calibrator.getEstimatedMyz());
21565         assertNull(calibrator.getEstimatedMzx());
21566         assertNull(calibrator.getEstimatedMzy());
21567         assertNull(calibrator.getEstimatedCovariance());
21568         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21569         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21570                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21571         assertNotNull(calibrator.getGroundTruthGravityNorm());
21572         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21573         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21574         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21575                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21576         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21577         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21578         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21579     }
21580 
21581     @Test
21582     public void testConstructor170() throws WrongSizeException {
21583         final Matrix ba = generateBa();
21584         final double biasX = ba.getElementAtIndex(0);
21585         final double biasY = ba.getElementAtIndex(1);
21586         final double biasZ = ba.getElementAtIndex(2);
21587 
21588         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21589         final double latitude = Math.toRadians(
21590                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21591         final double longitude = Math.toRadians(
21592                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21593         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21594         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21595         final NEDVelocity nedVelocity = new NEDVelocity();
21596         final ECEFPosition ecefPosition = new ECEFPosition();
21597         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21598         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21599                 ecefPosition, ecefVelocity);
21600 
21601         final KnownPositionAccelerometerCalibrator calibrator =
21602                 new KnownPositionAccelerometerCalibrator(nedPosition,
21603                         true, biasX, biasY, biasZ, this);
21604 
21605         // check default values
21606         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21607         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21608         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21609         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21610         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21611         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21612         final Acceleration bx2 = new Acceleration(0.0,
21613                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21614         calibrator.getInitialBiasXAsAcceleration(bx2);
21615         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21616         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21617         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21618         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21619         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21620         final Acceleration by2 = new Acceleration(0.0,
21621                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21622         calibrator.getInitialBiasYAsAcceleration(by2);
21623         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21624         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21625         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21626         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21627         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21628         final Acceleration bz2 = new Acceleration(0.0,
21629                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21630         calibrator.getInitialBiasZAsAcceleration(bz2);
21631         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21632         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21633         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21634         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21635         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21636         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21637         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21638         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21639         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21640         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21641         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21642         final double[] bias1 = calibrator.getInitialBias();
21643         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21644         final double[] bias2 = new double[3];
21645         calibrator.getInitialBias(bias2);
21646         assertArrayEquals(bias1, bias2, 0.0);
21647         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21648         assertEquals(b1, ba);
21649         final Matrix b2 = new Matrix(3, 1);
21650         calibrator.getInitialBiasAsMatrix(b2);
21651         assertEquals(b1, b2);
21652         final Matrix ma1 = calibrator.getInitialMa();
21653         assertEquals(ma1, new Matrix(3, 3));
21654         final Matrix ma2 = new Matrix(3, 3);
21655         calibrator.getInitialMa(ma2);
21656         assertEquals(ma1, ma2);
21657         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21658         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21659         final NEDPosition nedPosition1 = new NEDPosition();
21660         assertTrue(calibrator.getNedPosition(nedPosition1));
21661         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21662         assertNull(calibrator.getMeasurements());
21663         assertTrue(calibrator.isCommonAxisUsed());
21664         assertSame(calibrator.getListener(), this);
21665         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21666         assertFalse(calibrator.isReady());
21667         assertFalse(calibrator.isRunning());
21668         assertNull(calibrator.getEstimatedBiases());
21669         assertFalse(calibrator.getEstimatedBiases(null));
21670         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21671         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21672         assertNull(calibrator.getEstimatedBiasFx());
21673         assertNull(calibrator.getEstimatedBiasFy());
21674         assertNull(calibrator.getEstimatedBiasFz());
21675         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21676         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21677         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21678         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21679         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21680         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21681         assertNull(calibrator.getEstimatedMa());
21682         assertNull(calibrator.getEstimatedSx());
21683         assertNull(calibrator.getEstimatedSy());
21684         assertNull(calibrator.getEstimatedSz());
21685         assertNull(calibrator.getEstimatedMxy());
21686         assertNull(calibrator.getEstimatedMxz());
21687         assertNull(calibrator.getEstimatedMyx());
21688         assertNull(calibrator.getEstimatedMyz());
21689         assertNull(calibrator.getEstimatedMzx());
21690         assertNull(calibrator.getEstimatedMzy());
21691         assertNull(calibrator.getEstimatedCovariance());
21692         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21693         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21694                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21695         assertNotNull(calibrator.getGroundTruthGravityNorm());
21696         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21697         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21698         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21699                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21700         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21701         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21702         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21703     }
21704 
21705     @Test
21706     public void testConstructor171() throws WrongSizeException {
21707         final Collection<StandardDeviationBodyKinematics> measurements =
21708                 Collections.emptyList();
21709 
21710         final Matrix ba = generateBa();
21711         final double biasX = ba.getElementAtIndex(0);
21712         final double biasY = ba.getElementAtIndex(1);
21713         final double biasZ = ba.getElementAtIndex(2);
21714 
21715         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21716         final double latitude = Math.toRadians(
21717                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21718         final double longitude = Math.toRadians(
21719                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21720         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21721         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21722         final NEDVelocity nedVelocity = new NEDVelocity();
21723         final ECEFPosition ecefPosition = new ECEFPosition();
21724         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21725         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21726                 ecefPosition, ecefVelocity);
21727 
21728         final KnownPositionAccelerometerCalibrator calibrator =
21729                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21730                         true, biasX, biasY, biasZ);
21731 
21732         // check default values
21733         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21734         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21735         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21736         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21737         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21738         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21739         final Acceleration bx2 = new Acceleration(0.0,
21740                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21741         calibrator.getInitialBiasXAsAcceleration(bx2);
21742         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21743         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21744         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21745         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21746         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21747         final Acceleration by2 = new Acceleration(0.0,
21748                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21749         calibrator.getInitialBiasYAsAcceleration(by2);
21750         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21751         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21752         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21753         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21754         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21755         final Acceleration bz2 = new Acceleration(0.0,
21756                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21757         calibrator.getInitialBiasZAsAcceleration(bz2);
21758         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21759         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21760         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21761         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21762         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21763         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21764         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21765         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21766         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21767         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21768         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21769         final double[] bias1 = calibrator.getInitialBias();
21770         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21771         final double[] bias2 = new double[3];
21772         calibrator.getInitialBias(bias2);
21773         assertArrayEquals(bias1, bias2, 0.0);
21774         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21775         assertEquals(b1, ba);
21776         final Matrix b2 = new Matrix(3, 1);
21777         calibrator.getInitialBiasAsMatrix(b2);
21778         assertEquals(b1, b2);
21779         final Matrix ma1 = calibrator.getInitialMa();
21780         assertEquals(ma1, new Matrix(3, 3));
21781         final Matrix ma2 = new Matrix(3, 3);
21782         calibrator.getInitialMa(ma2);
21783         assertEquals(ma1, ma2);
21784         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21785         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21786         final NEDPosition nedPosition1 = new NEDPosition();
21787         assertTrue(calibrator.getNedPosition(nedPosition1));
21788         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21789         assertSame(calibrator.getMeasurements(), measurements);
21790         assertTrue(calibrator.isCommonAxisUsed());
21791         assertNull(calibrator.getListener());
21792         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21793         assertFalse(calibrator.isReady());
21794         assertFalse(calibrator.isRunning());
21795         assertNull(calibrator.getEstimatedBiases());
21796         assertFalse(calibrator.getEstimatedBiases(null));
21797         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21798         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21799         assertNull(calibrator.getEstimatedBiasFx());
21800         assertNull(calibrator.getEstimatedBiasFy());
21801         assertNull(calibrator.getEstimatedBiasFz());
21802         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21803         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21804         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21805         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21806         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21807         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21808         assertNull(calibrator.getEstimatedMa());
21809         assertNull(calibrator.getEstimatedSx());
21810         assertNull(calibrator.getEstimatedSy());
21811         assertNull(calibrator.getEstimatedSz());
21812         assertNull(calibrator.getEstimatedMxy());
21813         assertNull(calibrator.getEstimatedMxz());
21814         assertNull(calibrator.getEstimatedMyx());
21815         assertNull(calibrator.getEstimatedMyz());
21816         assertNull(calibrator.getEstimatedMzx());
21817         assertNull(calibrator.getEstimatedMzy());
21818         assertNull(calibrator.getEstimatedCovariance());
21819         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21820         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21821                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21822         assertNotNull(calibrator.getGroundTruthGravityNorm());
21823         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21824         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21825         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21826                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21827         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21828         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21829         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21830     }
21831 
21832     @Test
21833     public void testConstructor172() throws WrongSizeException {
21834         final Collection<StandardDeviationBodyKinematics> measurements =
21835                 Collections.emptyList();
21836 
21837         final Matrix ba = generateBa();
21838         final double biasX = ba.getElementAtIndex(0);
21839         final double biasY = ba.getElementAtIndex(1);
21840         final double biasZ = ba.getElementAtIndex(2);
21841 
21842         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21843         final double latitude = Math.toRadians(
21844                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21845         final double longitude = Math.toRadians(
21846                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21847         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21848         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21849         final NEDVelocity nedVelocity = new NEDVelocity();
21850         final ECEFPosition ecefPosition = new ECEFPosition();
21851         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21852         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21853                 ecefPosition, ecefVelocity);
21854 
21855         final KnownPositionAccelerometerCalibrator calibrator =
21856                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21857                         true, biasX, biasY, biasZ, this);
21858 
21859         // check default values
21860         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21861         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21862         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21863         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21864         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21865         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21866         final Acceleration bx2 = new Acceleration(0.0,
21867                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21868         calibrator.getInitialBiasXAsAcceleration(bx2);
21869         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21870         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21871         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21872         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21873         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21874         final Acceleration by2 = new Acceleration(0.0,
21875                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21876         calibrator.getInitialBiasYAsAcceleration(by2);
21877         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21878         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21879         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21880         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21881         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21882         final Acceleration bz2 = new Acceleration(0.0,
21883                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21884         calibrator.getInitialBiasZAsAcceleration(bz2);
21885         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21886         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21887         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21888         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21889         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21890         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21891         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21892         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21893         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21894         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21895         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21896         final double[] bias1 = calibrator.getInitialBias();
21897         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21898         final double[] bias2 = new double[3];
21899         calibrator.getInitialBias(bias2);
21900         assertArrayEquals(bias1, bias2, 0.0);
21901         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21902         assertEquals(b1, ba);
21903         final Matrix b2 = new Matrix(3, 1);
21904         calibrator.getInitialBiasAsMatrix(b2);
21905         assertEquals(b1, b2);
21906         final Matrix ma1 = calibrator.getInitialMa();
21907         assertEquals(ma1, new Matrix(3, 3));
21908         final Matrix ma2 = new Matrix(3, 3);
21909         calibrator.getInitialMa(ma2);
21910         assertEquals(ma1, ma2);
21911         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21912         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21913         final NEDPosition nedPosition1 = new NEDPosition();
21914         assertTrue(calibrator.getNedPosition(nedPosition1));
21915         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21916         assertSame(calibrator.getMeasurements(), measurements);
21917         assertTrue(calibrator.isCommonAxisUsed());
21918         assertSame(calibrator.getListener(), this);
21919         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21920         assertFalse(calibrator.isReady());
21921         assertFalse(calibrator.isRunning());
21922         assertNull(calibrator.getEstimatedBiases());
21923         assertFalse(calibrator.getEstimatedBiases(null));
21924         assertNull(calibrator.getEstimatedBiasesAsMatrix());
21925         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21926         assertNull(calibrator.getEstimatedBiasFx());
21927         assertNull(calibrator.getEstimatedBiasFy());
21928         assertNull(calibrator.getEstimatedBiasFz());
21929         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21930         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21931         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21932         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21933         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21934         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21935         assertNull(calibrator.getEstimatedMa());
21936         assertNull(calibrator.getEstimatedSx());
21937         assertNull(calibrator.getEstimatedSy());
21938         assertNull(calibrator.getEstimatedSz());
21939         assertNull(calibrator.getEstimatedMxy());
21940         assertNull(calibrator.getEstimatedMxz());
21941         assertNull(calibrator.getEstimatedMyx());
21942         assertNull(calibrator.getEstimatedMyz());
21943         assertNull(calibrator.getEstimatedMzx());
21944         assertNull(calibrator.getEstimatedMzy());
21945         assertNull(calibrator.getEstimatedCovariance());
21946         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21947         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21948                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21949         assertNotNull(calibrator.getGroundTruthGravityNorm());
21950         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21951         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21952         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21953                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21954         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21955         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21956         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21957     }
21958 
21959     @Test
21960     public void testConstructor173() throws WrongSizeException {
21961         final Matrix ba = generateBa();
21962         final double biasX = ba.getElementAtIndex(0);
21963         final double biasY = ba.getElementAtIndex(1);
21964         final double biasZ = ba.getElementAtIndex(2);
21965 
21966         final Acceleration bx = new Acceleration(biasX,
21967                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21968         final Acceleration by = new Acceleration(biasY,
21969                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21970         final Acceleration bz = new Acceleration(biasZ,
21971                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21972 
21973         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21974         final double latitude = Math.toRadians(
21975                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21976         final double longitude = Math.toRadians(
21977                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21978         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21979         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21980         final NEDVelocity nedVelocity = new NEDVelocity();
21981         final ECEFPosition ecefPosition = new ECEFPosition();
21982         final ECEFVelocity ecefVelocity = new ECEFVelocity();
21983         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21984                 ecefPosition, ecefVelocity);
21985 
21986         final KnownPositionAccelerometerCalibrator calibrator =
21987                 new KnownPositionAccelerometerCalibrator(nedPosition,
21988                         bx, by, bz);
21989 
21990         // check default values
21991         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21992         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21993         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21994         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21995         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21996         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21997         final Acceleration bx2 = new Acceleration(0.0,
21998                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21999         calibrator.getInitialBiasXAsAcceleration(bx2);
22000         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22001         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22002         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22003         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22004         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22005         final Acceleration by2 = new Acceleration(0.0,
22006                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22007         calibrator.getInitialBiasYAsAcceleration(by2);
22008         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22009         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22010         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22011         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22012         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22013         final Acceleration bz2 = new Acceleration(0.0,
22014                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22015         calibrator.getInitialBiasZAsAcceleration(bz2);
22016         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22017         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22018         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22019         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22020         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22021         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22022         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22023         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22024         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22025         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22026         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22027         final double[] bias1 = calibrator.getInitialBias();
22028         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22029         final double[] bias2 = new double[3];
22030         calibrator.getInitialBias(bias2);
22031         assertArrayEquals(bias1, bias2, 0.0);
22032         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22033         assertEquals(b1, ba);
22034         final Matrix b2 = new Matrix(3, 1);
22035         calibrator.getInitialBiasAsMatrix(b2);
22036         assertEquals(b1, b2);
22037         final Matrix ma1 = calibrator.getInitialMa();
22038         assertEquals(ma1, new Matrix(3, 3));
22039         final Matrix ma2 = new Matrix(3, 3);
22040         calibrator.getInitialMa(ma2);
22041         assertEquals(ma1, ma2);
22042         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22043         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22044         final NEDPosition nedPosition1 = new NEDPosition();
22045         assertTrue(calibrator.getNedPosition(nedPosition1));
22046         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22047         assertNull(calibrator.getMeasurements());
22048         assertFalse(calibrator.isCommonAxisUsed());
22049         assertNull(calibrator.getListener());
22050         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22051         assertFalse(calibrator.isReady());
22052         assertFalse(calibrator.isRunning());
22053         assertNull(calibrator.getEstimatedBiases());
22054         assertFalse(calibrator.getEstimatedBiases(null));
22055         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22056         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22057         assertNull(calibrator.getEstimatedBiasFx());
22058         assertNull(calibrator.getEstimatedBiasFy());
22059         assertNull(calibrator.getEstimatedBiasFz());
22060         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22061         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22062         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22063         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22064         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22065         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22066         assertNull(calibrator.getEstimatedMa());
22067         assertNull(calibrator.getEstimatedSx());
22068         assertNull(calibrator.getEstimatedSy());
22069         assertNull(calibrator.getEstimatedSz());
22070         assertNull(calibrator.getEstimatedMxy());
22071         assertNull(calibrator.getEstimatedMxz());
22072         assertNull(calibrator.getEstimatedMyx());
22073         assertNull(calibrator.getEstimatedMyz());
22074         assertNull(calibrator.getEstimatedMzx());
22075         assertNull(calibrator.getEstimatedMzy());
22076         assertNull(calibrator.getEstimatedCovariance());
22077         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22078         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22079                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22080         assertNotNull(calibrator.getGroundTruthGravityNorm());
22081         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22082         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22083         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22084                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22085         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22086         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22087         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22088     }
22089 
22090     @Test
22091     public void testConstructor174() throws WrongSizeException {
22092         final Matrix ba = generateBa();
22093         final double biasX = ba.getElementAtIndex(0);
22094         final double biasY = ba.getElementAtIndex(1);
22095         final double biasZ = ba.getElementAtIndex(2);
22096 
22097         final Acceleration bx = new Acceleration(biasX,
22098                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22099         final Acceleration by = new Acceleration(biasY,
22100                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22101         final Acceleration bz = new Acceleration(biasZ,
22102                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22103 
22104         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22105         final double latitude = Math.toRadians(
22106                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22107         final double longitude = Math.toRadians(
22108                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22109         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22110         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22111         final NEDVelocity nedVelocity = new NEDVelocity();
22112         final ECEFPosition ecefPosition = new ECEFPosition();
22113         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22114         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22115                 ecefPosition, ecefVelocity);
22116 
22117         final KnownPositionAccelerometerCalibrator calibrator =
22118                 new KnownPositionAccelerometerCalibrator(nedPosition,
22119                         bx, by, bz, this);
22120 
22121         // check default values
22122         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22123         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22124         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22125         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22126         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22127         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22128         final Acceleration bx2 = new Acceleration(0.0,
22129                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22130         calibrator.getInitialBiasXAsAcceleration(bx2);
22131         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22132         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22133         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22134         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22135         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22136         final Acceleration by2 = new Acceleration(0.0,
22137                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22138         calibrator.getInitialBiasYAsAcceleration(by2);
22139         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22140         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22141         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22142         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22143         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22144         final Acceleration bz2 = new Acceleration(0.0,
22145                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22146         calibrator.getInitialBiasZAsAcceleration(bz2);
22147         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22148         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22149         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22150         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22151         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22152         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22153         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22154         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22155         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22156         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22157         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22158         final double[] bias1 = calibrator.getInitialBias();
22159         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22160         final double[] bias2 = new double[3];
22161         calibrator.getInitialBias(bias2);
22162         assertArrayEquals(bias1, bias2, 0.0);
22163         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22164         assertEquals(b1, ba);
22165         final Matrix b2 = new Matrix(3, 1);
22166         calibrator.getInitialBiasAsMatrix(b2);
22167         assertEquals(b1, b2);
22168         final Matrix ma1 = calibrator.getInitialMa();
22169         assertEquals(ma1, new Matrix(3, 3));
22170         final Matrix ma2 = new Matrix(3, 3);
22171         calibrator.getInitialMa(ma2);
22172         assertEquals(ma1, ma2);
22173         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22174         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22175         final NEDPosition nedPosition1 = new NEDPosition();
22176         assertTrue(calibrator.getNedPosition(nedPosition1));
22177         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22178         assertNull(calibrator.getMeasurements());
22179         assertFalse(calibrator.isCommonAxisUsed());
22180         assertSame(calibrator.getListener(), this);
22181         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22182         assertFalse(calibrator.isReady());
22183         assertFalse(calibrator.isRunning());
22184         assertNull(calibrator.getEstimatedBiases());
22185         assertFalse(calibrator.getEstimatedBiases(null));
22186         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22187         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22188         assertNull(calibrator.getEstimatedBiasFx());
22189         assertNull(calibrator.getEstimatedBiasFy());
22190         assertNull(calibrator.getEstimatedBiasFz());
22191         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22192         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22193         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22194         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22195         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22196         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22197         assertNull(calibrator.getEstimatedMa());
22198         assertNull(calibrator.getEstimatedSx());
22199         assertNull(calibrator.getEstimatedSy());
22200         assertNull(calibrator.getEstimatedSz());
22201         assertNull(calibrator.getEstimatedMxy());
22202         assertNull(calibrator.getEstimatedMxz());
22203         assertNull(calibrator.getEstimatedMyx());
22204         assertNull(calibrator.getEstimatedMyz());
22205         assertNull(calibrator.getEstimatedMzx());
22206         assertNull(calibrator.getEstimatedMzy());
22207         assertNull(calibrator.getEstimatedCovariance());
22208         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22209         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22210                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22211         assertNotNull(calibrator.getGroundTruthGravityNorm());
22212         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22213         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22214         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22215                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22216         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22217         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22218         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22219     }
22220 
22221     @Test
22222     public void testConstructor175() throws WrongSizeException {
22223         final Collection<StandardDeviationBodyKinematics> measurements =
22224                 Collections.emptyList();
22225 
22226         final Matrix ba = generateBa();
22227         final double biasX = ba.getElementAtIndex(0);
22228         final double biasY = ba.getElementAtIndex(1);
22229         final double biasZ = ba.getElementAtIndex(2);
22230 
22231         final Acceleration bx = new Acceleration(biasX,
22232                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22233         final Acceleration by = new Acceleration(biasY,
22234                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22235         final Acceleration bz = new Acceleration(biasZ,
22236                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22237 
22238         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22239         final double latitude = Math.toRadians(
22240                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22241         final double longitude = Math.toRadians(
22242                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22243         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22244         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22245         final NEDVelocity nedVelocity = new NEDVelocity();
22246         final ECEFPosition ecefPosition = new ECEFPosition();
22247         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22248         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22249                 ecefPosition, ecefVelocity);
22250 
22251         final KnownPositionAccelerometerCalibrator calibrator =
22252                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22253                         bx, by, bz);
22254 
22255         // check default values
22256         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22257         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22258         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22259         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22260         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22261         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22262         final Acceleration bx2 = new Acceleration(0.0,
22263                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22264         calibrator.getInitialBiasXAsAcceleration(bx2);
22265         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22266         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22267         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22268         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22269         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22270         final Acceleration by2 = new Acceleration(0.0,
22271                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22272         calibrator.getInitialBiasYAsAcceleration(by2);
22273         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22274         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22275         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22276         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22277         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22278         final Acceleration bz2 = new Acceleration(0.0,
22279                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22280         calibrator.getInitialBiasZAsAcceleration(bz2);
22281         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22282         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22283         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22284         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22285         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22286         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22287         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22288         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22289         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22290         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22291         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22292         final double[] bias1 = calibrator.getInitialBias();
22293         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22294         final double[] bias2 = new double[3];
22295         calibrator.getInitialBias(bias2);
22296         assertArrayEquals(bias1, bias2, 0.0);
22297         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22298         assertEquals(b1, ba);
22299         final Matrix b2 = new Matrix(3, 1);
22300         calibrator.getInitialBiasAsMatrix(b2);
22301         assertEquals(b1, b2);
22302         final Matrix ma1 = calibrator.getInitialMa();
22303         assertEquals(ma1, new Matrix(3, 3));
22304         final Matrix ma2 = new Matrix(3, 3);
22305         calibrator.getInitialMa(ma2);
22306         assertEquals(ma1, ma2);
22307         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22308         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22309         final NEDPosition nedPosition1 = new NEDPosition();
22310         assertTrue(calibrator.getNedPosition(nedPosition1));
22311         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22312         assertSame(calibrator.getMeasurements(), measurements);
22313         assertFalse(calibrator.isCommonAxisUsed());
22314         assertNull(calibrator.getListener());
22315         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22316         assertFalse(calibrator.isReady());
22317         assertFalse(calibrator.isRunning());
22318         assertNull(calibrator.getEstimatedBiases());
22319         assertFalse(calibrator.getEstimatedBiases(null));
22320         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22321         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22322         assertNull(calibrator.getEstimatedBiasFx());
22323         assertNull(calibrator.getEstimatedBiasFy());
22324         assertNull(calibrator.getEstimatedBiasFz());
22325         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22326         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22327         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22328         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22329         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22330         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22331         assertNull(calibrator.getEstimatedMa());
22332         assertNull(calibrator.getEstimatedSx());
22333         assertNull(calibrator.getEstimatedSy());
22334         assertNull(calibrator.getEstimatedSz());
22335         assertNull(calibrator.getEstimatedMxy());
22336         assertNull(calibrator.getEstimatedMxz());
22337         assertNull(calibrator.getEstimatedMyx());
22338         assertNull(calibrator.getEstimatedMyz());
22339         assertNull(calibrator.getEstimatedMzx());
22340         assertNull(calibrator.getEstimatedMzy());
22341         assertNull(calibrator.getEstimatedCovariance());
22342         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22343         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22344                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22345         assertNotNull(calibrator.getGroundTruthGravityNorm());
22346         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22347         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22348         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22349                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22350         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22351         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22352         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22353     }
22354 
22355     @Test
22356     public void testConstructor176() throws WrongSizeException {
22357         final Collection<StandardDeviationBodyKinematics> measurements =
22358                 Collections.emptyList();
22359 
22360         final Matrix ba = generateBa();
22361         final double biasX = ba.getElementAtIndex(0);
22362         final double biasY = ba.getElementAtIndex(1);
22363         final double biasZ = ba.getElementAtIndex(2);
22364 
22365         final Acceleration bx = new Acceleration(biasX,
22366                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22367         final Acceleration by = new Acceleration(biasY,
22368                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22369         final Acceleration bz = new Acceleration(biasZ,
22370                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22371 
22372         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22373         final double latitude = Math.toRadians(
22374                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22375         final double longitude = Math.toRadians(
22376                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22377         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22378         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22379         final NEDVelocity nedVelocity = new NEDVelocity();
22380         final ECEFPosition ecefPosition = new ECEFPosition();
22381         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22382         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22383                 ecefPosition, ecefVelocity);
22384 
22385         final KnownPositionAccelerometerCalibrator calibrator =
22386                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22387                         bx, by, bz, this);
22388 
22389         // check default values
22390         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22391         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22392         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22393         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22394         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22395         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22396         final Acceleration bx2 = new Acceleration(0.0,
22397                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22398         calibrator.getInitialBiasXAsAcceleration(bx2);
22399         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22400         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22401         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22402         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22403         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22404         final Acceleration by2 = new Acceleration(0.0,
22405                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22406         calibrator.getInitialBiasYAsAcceleration(by2);
22407         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22408         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22409         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22410         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22411         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22412         final Acceleration bz2 = new Acceleration(0.0,
22413                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22414         calibrator.getInitialBiasZAsAcceleration(bz2);
22415         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22416         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22417         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22418         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22419         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22420         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22421         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22422         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22423         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22424         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22425         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22426         final double[] bias1 = calibrator.getInitialBias();
22427         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22428         final double[] bias2 = new double[3];
22429         calibrator.getInitialBias(bias2);
22430         assertArrayEquals(bias1, bias2, 0.0);
22431         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22432         assertEquals(b1, ba);
22433         final Matrix b2 = new Matrix(3, 1);
22434         calibrator.getInitialBiasAsMatrix(b2);
22435         assertEquals(b1, b2);
22436         final Matrix ma1 = calibrator.getInitialMa();
22437         assertEquals(ma1, new Matrix(3, 3));
22438         final Matrix ma2 = new Matrix(3, 3);
22439         calibrator.getInitialMa(ma2);
22440         assertEquals(ma1, ma2);
22441         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22442         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22443         final NEDPosition nedPosition1 = new NEDPosition();
22444         assertTrue(calibrator.getNedPosition(nedPosition1));
22445         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22446         assertSame(calibrator.getMeasurements(), measurements);
22447         assertFalse(calibrator.isCommonAxisUsed());
22448         assertSame(calibrator.getListener(), this);
22449         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22450         assertFalse(calibrator.isReady());
22451         assertFalse(calibrator.isRunning());
22452         assertNull(calibrator.getEstimatedBiases());
22453         assertFalse(calibrator.getEstimatedBiases(null));
22454         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22455         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22456         assertNull(calibrator.getEstimatedBiasFx());
22457         assertNull(calibrator.getEstimatedBiasFy());
22458         assertNull(calibrator.getEstimatedBiasFz());
22459         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22460         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22461         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22462         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22463         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22464         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22465         assertNull(calibrator.getEstimatedMa());
22466         assertNull(calibrator.getEstimatedSx());
22467         assertNull(calibrator.getEstimatedSy());
22468         assertNull(calibrator.getEstimatedSz());
22469         assertNull(calibrator.getEstimatedMxy());
22470         assertNull(calibrator.getEstimatedMxz());
22471         assertNull(calibrator.getEstimatedMyx());
22472         assertNull(calibrator.getEstimatedMyz());
22473         assertNull(calibrator.getEstimatedMzx());
22474         assertNull(calibrator.getEstimatedMzy());
22475         assertNull(calibrator.getEstimatedCovariance());
22476         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22477         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22478                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22479         assertNotNull(calibrator.getGroundTruthGravityNorm());
22480         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22481         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22482         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22483                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22484         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22485         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22486         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22487     }
22488 
22489     @Test
22490     public void testConstructor177() throws WrongSizeException {
22491         final Matrix ba = generateBa();
22492         final double biasX = ba.getElementAtIndex(0);
22493         final double biasY = ba.getElementAtIndex(1);
22494         final double biasZ = ba.getElementAtIndex(2);
22495 
22496         final Acceleration bx = new Acceleration(biasX,
22497                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22498         final Acceleration by = new Acceleration(biasY,
22499                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22500         final Acceleration bz = new Acceleration(biasZ,
22501                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22502 
22503         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22504         final double latitude = Math.toRadians(
22505                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22506         final double longitude = Math.toRadians(
22507                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22508         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22509         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22510         final NEDVelocity nedVelocity = new NEDVelocity();
22511         final ECEFPosition ecefPosition = new ECEFPosition();
22512         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22513         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22514                 ecefPosition, ecefVelocity);
22515 
22516         final KnownPositionAccelerometerCalibrator calibrator =
22517                 new KnownPositionAccelerometerCalibrator(nedPosition,
22518                         true, bx, by, bz);
22519 
22520         // check default values
22521         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22522         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22523         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22524         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22525         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22526         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22527         final Acceleration bx2 = new Acceleration(0.0,
22528                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22529         calibrator.getInitialBiasXAsAcceleration(bx2);
22530         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22531         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22532         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22533         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22534         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22535         final Acceleration by2 = new Acceleration(0.0,
22536                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22537         calibrator.getInitialBiasYAsAcceleration(by2);
22538         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22539         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22540         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22541         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22542         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22543         final Acceleration bz2 = new Acceleration(0.0,
22544                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22545         calibrator.getInitialBiasZAsAcceleration(bz2);
22546         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22547         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22548         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22549         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22550         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22551         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22552         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22553         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22554         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22555         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22556         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22557         final double[] bias1 = calibrator.getInitialBias();
22558         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22559         final double[] bias2 = new double[3];
22560         calibrator.getInitialBias(bias2);
22561         assertArrayEquals(bias1, bias2, 0.0);
22562         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22563         assertEquals(b1, ba);
22564         final Matrix b2 = new Matrix(3, 1);
22565         calibrator.getInitialBiasAsMatrix(b2);
22566         assertEquals(b1, b2);
22567         final Matrix ma1 = calibrator.getInitialMa();
22568         assertEquals(ma1, new Matrix(3, 3));
22569         final Matrix ma2 = new Matrix(3, 3);
22570         calibrator.getInitialMa(ma2);
22571         assertEquals(ma1, ma2);
22572         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22573         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22574         final NEDPosition nedPosition1 = new NEDPosition();
22575         assertTrue(calibrator.getNedPosition(nedPosition1));
22576         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22577         assertNull(calibrator.getMeasurements());
22578         assertTrue(calibrator.isCommonAxisUsed());
22579         assertNull(calibrator.getListener());
22580         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22581         assertFalse(calibrator.isReady());
22582         assertFalse(calibrator.isRunning());
22583         assertNull(calibrator.getEstimatedBiases());
22584         assertFalse(calibrator.getEstimatedBiases(null));
22585         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22586         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22587         assertNull(calibrator.getEstimatedBiasFx());
22588         assertNull(calibrator.getEstimatedBiasFy());
22589         assertNull(calibrator.getEstimatedBiasFz());
22590         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22591         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22592         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22593         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22594         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22595         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22596         assertNull(calibrator.getEstimatedMa());
22597         assertNull(calibrator.getEstimatedSx());
22598         assertNull(calibrator.getEstimatedSy());
22599         assertNull(calibrator.getEstimatedSz());
22600         assertNull(calibrator.getEstimatedMxy());
22601         assertNull(calibrator.getEstimatedMxz());
22602         assertNull(calibrator.getEstimatedMyx());
22603         assertNull(calibrator.getEstimatedMyz());
22604         assertNull(calibrator.getEstimatedMzx());
22605         assertNull(calibrator.getEstimatedMzy());
22606         assertNull(calibrator.getEstimatedCovariance());
22607         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22608         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22609                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22610         assertNotNull(calibrator.getGroundTruthGravityNorm());
22611         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22612         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22613         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22614                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22615         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22616         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22617         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22618     }
22619 
22620     @Test
22621     public void testConstructor178() throws WrongSizeException {
22622         final Matrix ba = generateBa();
22623         final double biasX = ba.getElementAtIndex(0);
22624         final double biasY = ba.getElementAtIndex(1);
22625         final double biasZ = ba.getElementAtIndex(2);
22626 
22627         final Acceleration bx = new Acceleration(biasX,
22628                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22629         final Acceleration by = new Acceleration(biasY,
22630                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22631         final Acceleration bz = new Acceleration(biasZ,
22632                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22633 
22634         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22635         final double latitude = Math.toRadians(
22636                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22637         final double longitude = Math.toRadians(
22638                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22639         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22640         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22641         final NEDVelocity nedVelocity = new NEDVelocity();
22642         final ECEFPosition ecefPosition = new ECEFPosition();
22643         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22644         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22645                 ecefPosition, ecefVelocity);
22646 
22647         final KnownPositionAccelerometerCalibrator calibrator =
22648                 new KnownPositionAccelerometerCalibrator(nedPosition,
22649                         true, bx, by, bz, this);
22650 
22651         // check default values
22652         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22653         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22654         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22655         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22656         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22657         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22658         final Acceleration bx2 = new Acceleration(0.0,
22659                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22660         calibrator.getInitialBiasXAsAcceleration(bx2);
22661         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22662         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22663         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22664         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22665         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22666         final Acceleration by2 = new Acceleration(0.0,
22667                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22668         calibrator.getInitialBiasYAsAcceleration(by2);
22669         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22670         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22671         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22672         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22673         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22674         final Acceleration bz2 = new Acceleration(0.0,
22675                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22676         calibrator.getInitialBiasZAsAcceleration(bz2);
22677         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22678         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22679         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22680         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22681         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22682         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22683         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22684         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22685         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22686         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22687         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22688         final double[] bias1 = calibrator.getInitialBias();
22689         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22690         final double[] bias2 = new double[3];
22691         calibrator.getInitialBias(bias2);
22692         assertArrayEquals(bias1, bias2, 0.0);
22693         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22694         assertEquals(b1, ba);
22695         final Matrix b2 = new Matrix(3, 1);
22696         calibrator.getInitialBiasAsMatrix(b2);
22697         assertEquals(b1, b2);
22698         final Matrix ma1 = calibrator.getInitialMa();
22699         assertEquals(ma1, new Matrix(3, 3));
22700         final Matrix ma2 = new Matrix(3, 3);
22701         calibrator.getInitialMa(ma2);
22702         assertEquals(ma1, ma2);
22703         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22704         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22705         final NEDPosition nedPosition1 = new NEDPosition();
22706         assertTrue(calibrator.getNedPosition(nedPosition1));
22707         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22708         assertNull(calibrator.getMeasurements());
22709         assertTrue(calibrator.isCommonAxisUsed());
22710         assertSame(calibrator.getListener(), this);
22711         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22712         assertFalse(calibrator.isReady());
22713         assertFalse(calibrator.isRunning());
22714         assertNull(calibrator.getEstimatedBiases());
22715         assertFalse(calibrator.getEstimatedBiases(null));
22716         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22717         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22718         assertNull(calibrator.getEstimatedBiasFx());
22719         assertNull(calibrator.getEstimatedBiasFy());
22720         assertNull(calibrator.getEstimatedBiasFz());
22721         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22722         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22723         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22724         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22725         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22726         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22727         assertNull(calibrator.getEstimatedMa());
22728         assertNull(calibrator.getEstimatedSx());
22729         assertNull(calibrator.getEstimatedSy());
22730         assertNull(calibrator.getEstimatedSz());
22731         assertNull(calibrator.getEstimatedMxy());
22732         assertNull(calibrator.getEstimatedMxz());
22733         assertNull(calibrator.getEstimatedMyx());
22734         assertNull(calibrator.getEstimatedMyz());
22735         assertNull(calibrator.getEstimatedMzx());
22736         assertNull(calibrator.getEstimatedMzy());
22737         assertNull(calibrator.getEstimatedCovariance());
22738         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22739         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22740                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22741         assertNotNull(calibrator.getGroundTruthGravityNorm());
22742         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22743         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22744         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22745                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22746         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22747         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22748         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22749     }
22750 
22751     @Test
22752     public void testConstructor179() throws WrongSizeException {
22753         final Collection<StandardDeviationBodyKinematics> measurements =
22754                 Collections.emptyList();
22755 
22756         final Matrix ba = generateBa();
22757         final double biasX = ba.getElementAtIndex(0);
22758         final double biasY = ba.getElementAtIndex(1);
22759         final double biasZ = ba.getElementAtIndex(2);
22760 
22761         final Acceleration bx = new Acceleration(biasX,
22762                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22763         final Acceleration by = new Acceleration(biasY,
22764                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22765         final Acceleration bz = new Acceleration(biasZ,
22766                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22767 
22768         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22769         final double latitude = Math.toRadians(
22770                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22771         final double longitude = Math.toRadians(
22772                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22773         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22774         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22775         final NEDVelocity nedVelocity = new NEDVelocity();
22776         final ECEFPosition ecefPosition = new ECEFPosition();
22777         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22778         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22779                 ecefPosition, ecefVelocity);
22780 
22781         final KnownPositionAccelerometerCalibrator calibrator =
22782                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22783                         true, bx, by, bz);
22784 
22785         // check default values
22786         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22787         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22788         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22789         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22790         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22791         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22792         final Acceleration bx2 = new Acceleration(0.0,
22793                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22794         calibrator.getInitialBiasXAsAcceleration(bx2);
22795         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22796         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22797         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22798         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22799         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22800         final Acceleration by2 = new Acceleration(0.0,
22801                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22802         calibrator.getInitialBiasYAsAcceleration(by2);
22803         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22804         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22805         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22806         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22807         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22808         final Acceleration bz2 = new Acceleration(0.0,
22809                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22810         calibrator.getInitialBiasZAsAcceleration(bz2);
22811         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22812         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22813         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22814         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22815         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22816         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22817         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22818         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22819         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22820         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22821         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22822         final double[] bias1 = calibrator.getInitialBias();
22823         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22824         final double[] bias2 = new double[3];
22825         calibrator.getInitialBias(bias2);
22826         assertArrayEquals(bias1, bias2, 0.0);
22827         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22828         assertEquals(b1, ba);
22829         final Matrix b2 = new Matrix(3, 1);
22830         calibrator.getInitialBiasAsMatrix(b2);
22831         assertEquals(b1, b2);
22832         final Matrix ma1 = calibrator.getInitialMa();
22833         assertEquals(ma1, new Matrix(3, 3));
22834         final Matrix ma2 = new Matrix(3, 3);
22835         calibrator.getInitialMa(ma2);
22836         assertEquals(ma1, ma2);
22837         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22838         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22839         final NEDPosition nedPosition1 = new NEDPosition();
22840         assertTrue(calibrator.getNedPosition(nedPosition1));
22841         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22842         assertSame(calibrator.getMeasurements(), measurements);
22843         assertTrue(calibrator.isCommonAxisUsed());
22844         assertNull(calibrator.getListener());
22845         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22846         assertFalse(calibrator.isReady());
22847         assertFalse(calibrator.isRunning());
22848         assertNull(calibrator.getEstimatedBiases());
22849         assertFalse(calibrator.getEstimatedBiases(null));
22850         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22851         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22852         assertNull(calibrator.getEstimatedBiasFx());
22853         assertNull(calibrator.getEstimatedBiasFy());
22854         assertNull(calibrator.getEstimatedBiasFz());
22855         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22856         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22857         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22858         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22859         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22860         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22861         assertNull(calibrator.getEstimatedMa());
22862         assertNull(calibrator.getEstimatedSx());
22863         assertNull(calibrator.getEstimatedSy());
22864         assertNull(calibrator.getEstimatedSz());
22865         assertNull(calibrator.getEstimatedMxy());
22866         assertNull(calibrator.getEstimatedMxz());
22867         assertNull(calibrator.getEstimatedMyx());
22868         assertNull(calibrator.getEstimatedMyz());
22869         assertNull(calibrator.getEstimatedMzx());
22870         assertNull(calibrator.getEstimatedMzy());
22871         assertNull(calibrator.getEstimatedCovariance());
22872         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22873         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22874                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22875         assertNotNull(calibrator.getGroundTruthGravityNorm());
22876         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22877         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22878         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22879                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22880         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22881         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22882         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22883     }
22884 
22885     @Test
22886     public void testConstructor180() throws WrongSizeException {
22887         final Collection<StandardDeviationBodyKinematics> measurements =
22888                 Collections.emptyList();
22889 
22890         final Matrix ba = generateBa();
22891         final double biasX = ba.getElementAtIndex(0);
22892         final double biasY = ba.getElementAtIndex(1);
22893         final double biasZ = ba.getElementAtIndex(2);
22894 
22895         final Acceleration bx = new Acceleration(biasX,
22896                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22897         final Acceleration by = new Acceleration(biasY,
22898                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22899         final Acceleration bz = new Acceleration(biasZ,
22900                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22901 
22902         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22903         final double latitude = Math.toRadians(
22904                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22905         final double longitude = Math.toRadians(
22906                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22907         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22908         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22909         final NEDVelocity nedVelocity = new NEDVelocity();
22910         final ECEFPosition ecefPosition = new ECEFPosition();
22911         final ECEFVelocity ecefVelocity = new ECEFVelocity();
22912         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22913                 ecefPosition, ecefVelocity);
22914 
22915         final KnownPositionAccelerometerCalibrator calibrator =
22916                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22917                         true, bx, by, bz, this);
22918 
22919         // check default values
22920         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22921         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22922         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22923         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22924         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22925         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22926         final Acceleration bx2 = new Acceleration(0.0,
22927                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22928         calibrator.getInitialBiasXAsAcceleration(bx2);
22929         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22930         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22931         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22932         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22933         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22934         final Acceleration by2 = new Acceleration(0.0,
22935                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22936         calibrator.getInitialBiasYAsAcceleration(by2);
22937         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22938         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22939         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22940         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22941         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22942         final Acceleration bz2 = new Acceleration(0.0,
22943                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22944         calibrator.getInitialBiasZAsAcceleration(bz2);
22945         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22946         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22947         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22948         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22949         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22950         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22951         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22952         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22953         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22954         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22955         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22956         final double[] bias1 = calibrator.getInitialBias();
22957         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22958         final double[] bias2 = new double[3];
22959         calibrator.getInitialBias(bias2);
22960         assertArrayEquals(bias1, bias2, 0.0);
22961         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22962         assertEquals(b1, ba);
22963         final Matrix b2 = new Matrix(3, 1);
22964         calibrator.getInitialBiasAsMatrix(b2);
22965         assertEquals(b1, b2);
22966         final Matrix ma1 = calibrator.getInitialMa();
22967         assertEquals(ma1, new Matrix(3, 3));
22968         final Matrix ma2 = new Matrix(3, 3);
22969         calibrator.getInitialMa(ma2);
22970         assertEquals(ma1, ma2);
22971         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22972         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22973         final NEDPosition nedPosition1 = new NEDPosition();
22974         assertTrue(calibrator.getNedPosition(nedPosition1));
22975         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22976         assertSame(calibrator.getMeasurements(), measurements);
22977         assertTrue(calibrator.isCommonAxisUsed());
22978         assertSame(calibrator.getListener(), this);
22979         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22980         assertFalse(calibrator.isReady());
22981         assertFalse(calibrator.isRunning());
22982         assertNull(calibrator.getEstimatedBiases());
22983         assertFalse(calibrator.getEstimatedBiases(null));
22984         assertNull(calibrator.getEstimatedBiasesAsMatrix());
22985         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22986         assertNull(calibrator.getEstimatedBiasFx());
22987         assertNull(calibrator.getEstimatedBiasFy());
22988         assertNull(calibrator.getEstimatedBiasFz());
22989         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22990         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22991         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22992         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22993         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22994         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22995         assertNull(calibrator.getEstimatedMa());
22996         assertNull(calibrator.getEstimatedSx());
22997         assertNull(calibrator.getEstimatedSy());
22998         assertNull(calibrator.getEstimatedSz());
22999         assertNull(calibrator.getEstimatedMxy());
23000         assertNull(calibrator.getEstimatedMxz());
23001         assertNull(calibrator.getEstimatedMyx());
23002         assertNull(calibrator.getEstimatedMyz());
23003         assertNull(calibrator.getEstimatedMzx());
23004         assertNull(calibrator.getEstimatedMzy());
23005         assertNull(calibrator.getEstimatedCovariance());
23006         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23007         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23008                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23009         assertNotNull(calibrator.getGroundTruthGravityNorm());
23010         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23011         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23012         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23013                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23014         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23015         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23016         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23017     }
23018 
23019     @Test
23020     public void testConstructor181() throws WrongSizeException {
23021         final Matrix ba = generateBa();
23022         final double biasX = ba.getElementAtIndex(0);
23023         final double biasY = ba.getElementAtIndex(1);
23024         final double biasZ = ba.getElementAtIndex(2);
23025 
23026         final Matrix ma = generateMaCommonAxis();
23027         final double sx = ma.getElementAt(0, 0);
23028         final double sy = ma.getElementAt(1, 1);
23029         final double sz = ma.getElementAt(2, 2);
23030 
23031         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23032         final double latitude = Math.toRadians(
23033                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23034         final double longitude = Math.toRadians(
23035                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23036         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23037         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23038         final NEDVelocity nedVelocity = new NEDVelocity();
23039         final ECEFPosition ecefPosition = new ECEFPosition();
23040         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23041         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23042                 ecefPosition, ecefVelocity);
23043 
23044         final KnownPositionAccelerometerCalibrator calibrator =
23045                 new KnownPositionAccelerometerCalibrator(nedPosition,
23046                         biasX, biasY, biasZ, sx, sy, sz);
23047 
23048         // check default values
23049         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23050         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23051         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23052         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23053         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23054         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23055         final Acceleration bx2 = new Acceleration(0.0,
23056                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23057         calibrator.getInitialBiasXAsAcceleration(bx2);
23058         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23059         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23060         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23061         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23062         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23063         final Acceleration by2 = new Acceleration(0.0,
23064                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23065         calibrator.getInitialBiasYAsAcceleration(by2);
23066         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23067         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23068         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23069         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23070         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23071         final Acceleration bz2 = new Acceleration(0.0,
23072                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23073         calibrator.getInitialBiasZAsAcceleration(bz2);
23074         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23075         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23076         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23077         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23078         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23079         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23080         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23081         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23082         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23083         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23084         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23085         final double[] bias1 = calibrator.getInitialBias();
23086         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23087         final double[] bias2 = new double[3];
23088         calibrator.getInitialBias(bias2);
23089         assertArrayEquals(bias1, bias2, 0.0);
23090         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23091         assertEquals(b1, ba);
23092         final Matrix b2 = new Matrix(3, 1);
23093         calibrator.getInitialBiasAsMatrix(b2);
23094         assertEquals(b1, b2);
23095         final Matrix ma1 = calibrator.getInitialMa();
23096         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23097         final Matrix ma2 = new Matrix(3, 3);
23098         calibrator.getInitialMa(ma2);
23099         assertEquals(ma1, ma2);
23100         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23101         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23102         final NEDPosition nedPosition1 = new NEDPosition();
23103         assertTrue(calibrator.getNedPosition(nedPosition1));
23104         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23105         assertNull(calibrator.getMeasurements());
23106         assertFalse(calibrator.isCommonAxisUsed());
23107         assertNull(calibrator.getListener());
23108         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23109         assertFalse(calibrator.isReady());
23110         assertFalse(calibrator.isRunning());
23111         assertNull(calibrator.getEstimatedBiases());
23112         assertFalse(calibrator.getEstimatedBiases(null));
23113         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23114         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23115         assertNull(calibrator.getEstimatedBiasFx());
23116         assertNull(calibrator.getEstimatedBiasFy());
23117         assertNull(calibrator.getEstimatedBiasFz());
23118         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23119         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23120         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23121         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23122         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23123         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23124         assertNull(calibrator.getEstimatedMa());
23125         assertNull(calibrator.getEstimatedSx());
23126         assertNull(calibrator.getEstimatedSy());
23127         assertNull(calibrator.getEstimatedSz());
23128         assertNull(calibrator.getEstimatedMxy());
23129         assertNull(calibrator.getEstimatedMxz());
23130         assertNull(calibrator.getEstimatedMyx());
23131         assertNull(calibrator.getEstimatedMyz());
23132         assertNull(calibrator.getEstimatedMzx());
23133         assertNull(calibrator.getEstimatedMzy());
23134         assertNull(calibrator.getEstimatedCovariance());
23135         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23136         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23137                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23138         assertNotNull(calibrator.getGroundTruthGravityNorm());
23139         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23140         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23141         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23142                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23143         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23144         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23145         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23146     }
23147 
23148     @Test
23149     public void testConstructor182() throws WrongSizeException {
23150         final Collection<StandardDeviationBodyKinematics> measurements =
23151                 Collections.emptyList();
23152 
23153         final Matrix ba = generateBa();
23154         final double biasX = ba.getElementAtIndex(0);
23155         final double biasY = ba.getElementAtIndex(1);
23156         final double biasZ = ba.getElementAtIndex(2);
23157 
23158         final Matrix ma = generateMaCommonAxis();
23159         final double sx = ma.getElementAt(0, 0);
23160         final double sy = ma.getElementAt(1, 1);
23161         final double sz = ma.getElementAt(2, 2);
23162 
23163         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23164         final double latitude = Math.toRadians(
23165                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23166         final double longitude = Math.toRadians(
23167                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23168         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23169         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23170         final NEDVelocity nedVelocity = new NEDVelocity();
23171         final ECEFPosition ecefPosition = new ECEFPosition();
23172         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23173         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23174                 ecefPosition, ecefVelocity);
23175 
23176         final KnownPositionAccelerometerCalibrator calibrator =
23177                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23178                         biasX, biasY, biasZ, sx, sy, sz);
23179 
23180         // check default values
23181         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23182         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23183         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23184         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23185         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23186         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23187         final Acceleration bx2 = new Acceleration(0.0,
23188                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23189         calibrator.getInitialBiasXAsAcceleration(bx2);
23190         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23191         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23192         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23193         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23194         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23195         final Acceleration by2 = new Acceleration(0.0,
23196                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23197         calibrator.getInitialBiasYAsAcceleration(by2);
23198         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23199         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23200         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23201         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23202         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23203         final Acceleration bz2 = new Acceleration(0.0,
23204                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23205         calibrator.getInitialBiasZAsAcceleration(bz2);
23206         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23207         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23208         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23209         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23210         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23211         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23212         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23213         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23214         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23215         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23216         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23217         final double[] bias1 = calibrator.getInitialBias();
23218         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23219         final double[] bias2 = new double[3];
23220         calibrator.getInitialBias(bias2);
23221         assertArrayEquals(bias1, bias2, 0.0);
23222         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23223         assertEquals(b1, ba);
23224         final Matrix b2 = new Matrix(3, 1);
23225         calibrator.getInitialBiasAsMatrix(b2);
23226         assertEquals(b1, b2);
23227         final Matrix ma1 = calibrator.getInitialMa();
23228         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23229         final Matrix ma2 = new Matrix(3, 3);
23230         calibrator.getInitialMa(ma2);
23231         assertEquals(ma1, ma2);
23232         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23233         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23234         final NEDPosition nedPosition1 = new NEDPosition();
23235         assertTrue(calibrator.getNedPosition(nedPosition1));
23236         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23237         assertSame(calibrator.getMeasurements(), measurements);
23238         assertFalse(calibrator.isCommonAxisUsed());
23239         assertNull(calibrator.getListener());
23240         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23241         assertFalse(calibrator.isReady());
23242         assertFalse(calibrator.isRunning());
23243         assertNull(calibrator.getEstimatedBiases());
23244         assertFalse(calibrator.getEstimatedBiases(null));
23245         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23246         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23247         assertNull(calibrator.getEstimatedBiasFx());
23248         assertNull(calibrator.getEstimatedBiasFy());
23249         assertNull(calibrator.getEstimatedBiasFz());
23250         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23251         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23252         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23253         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23254         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23255         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23256         assertNull(calibrator.getEstimatedMa());
23257         assertNull(calibrator.getEstimatedSx());
23258         assertNull(calibrator.getEstimatedSy());
23259         assertNull(calibrator.getEstimatedSz());
23260         assertNull(calibrator.getEstimatedMxy());
23261         assertNull(calibrator.getEstimatedMxz());
23262         assertNull(calibrator.getEstimatedMyx());
23263         assertNull(calibrator.getEstimatedMyz());
23264         assertNull(calibrator.getEstimatedMzx());
23265         assertNull(calibrator.getEstimatedMzy());
23266         assertNull(calibrator.getEstimatedCovariance());
23267         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23268         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23269                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23270         assertNotNull(calibrator.getGroundTruthGravityNorm());
23271         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23272         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23273         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23274                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23275         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23276         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23277         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23278     }
23279 
23280     @Test
23281     public void testConstructor183() throws WrongSizeException {
23282         final Collection<StandardDeviationBodyKinematics> measurements =
23283                 Collections.emptyList();
23284 
23285         final Matrix ba = generateBa();
23286         final double biasX = ba.getElementAtIndex(0);
23287         final double biasY = ba.getElementAtIndex(1);
23288         final double biasZ = ba.getElementAtIndex(2);
23289 
23290         final Matrix ma = generateMaCommonAxis();
23291         final double sx = ma.getElementAt(0, 0);
23292         final double sy = ma.getElementAt(1, 1);
23293         final double sz = ma.getElementAt(2, 2);
23294 
23295         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23296         final double latitude = Math.toRadians(
23297                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23298         final double longitude = Math.toRadians(
23299                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23300         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23301         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23302         final NEDVelocity nedVelocity = new NEDVelocity();
23303         final ECEFPosition ecefPosition = new ECEFPosition();
23304         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23305         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23306                 ecefPosition, ecefVelocity);
23307 
23308         final KnownPositionAccelerometerCalibrator calibrator =
23309                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23310                         biasX, biasY, biasZ, sx, sy, sz, this);
23311 
23312         // check default values
23313         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23314         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23315         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23316         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23317         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23318         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23319         final Acceleration bx2 = new Acceleration(0.0,
23320                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23321         calibrator.getInitialBiasXAsAcceleration(bx2);
23322         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23323         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23324         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23325         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23326         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23327         final Acceleration by2 = new Acceleration(0.0,
23328                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23329         calibrator.getInitialBiasYAsAcceleration(by2);
23330         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23331         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23332         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23333         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23334         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23335         final Acceleration bz2 = new Acceleration(0.0,
23336                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23337         calibrator.getInitialBiasZAsAcceleration(bz2);
23338         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23339         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23340         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23341         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23342         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23343         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23344         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23345         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23346         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23347         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23348         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23349         final double[] bias1 = calibrator.getInitialBias();
23350         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23351         final double[] bias2 = new double[3];
23352         calibrator.getInitialBias(bias2);
23353         assertArrayEquals(bias1, bias2, 0.0);
23354         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23355         assertEquals(b1, ba);
23356         final Matrix b2 = new Matrix(3, 1);
23357         calibrator.getInitialBiasAsMatrix(b2);
23358         assertEquals(b1, b2);
23359         final Matrix ma1 = calibrator.getInitialMa();
23360         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23361         final Matrix ma2 = new Matrix(3, 3);
23362         calibrator.getInitialMa(ma2);
23363         assertEquals(ma1, ma2);
23364         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23365         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23366         final NEDPosition nedPosition1 = new NEDPosition();
23367         assertTrue(calibrator.getNedPosition(nedPosition1));
23368         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23369         assertSame(calibrator.getMeasurements(), measurements);
23370         assertFalse(calibrator.isCommonAxisUsed());
23371         assertSame(calibrator.getListener(), this);
23372         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23373         assertFalse(calibrator.isReady());
23374         assertFalse(calibrator.isRunning());
23375         assertNull(calibrator.getEstimatedBiases());
23376         assertFalse(calibrator.getEstimatedBiases(null));
23377         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23378         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23379         assertNull(calibrator.getEstimatedBiasFx());
23380         assertNull(calibrator.getEstimatedBiasFy());
23381         assertNull(calibrator.getEstimatedBiasFz());
23382         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23383         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23384         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23385         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23386         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23387         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23388         assertNull(calibrator.getEstimatedMa());
23389         assertNull(calibrator.getEstimatedSx());
23390         assertNull(calibrator.getEstimatedSy());
23391         assertNull(calibrator.getEstimatedSz());
23392         assertNull(calibrator.getEstimatedMxy());
23393         assertNull(calibrator.getEstimatedMxz());
23394         assertNull(calibrator.getEstimatedMyx());
23395         assertNull(calibrator.getEstimatedMyz());
23396         assertNull(calibrator.getEstimatedMzx());
23397         assertNull(calibrator.getEstimatedMzy());
23398         assertNull(calibrator.getEstimatedCovariance());
23399         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23400         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23401                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23402         assertNotNull(calibrator.getGroundTruthGravityNorm());
23403         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23404         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23405         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23406                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23407         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23408         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23409         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23410     }
23411 
23412     @Test
23413     public void testConstructor184() throws WrongSizeException {
23414         final Matrix ba = generateBa();
23415         final double biasX = ba.getElementAtIndex(0);
23416         final double biasY = ba.getElementAtIndex(1);
23417         final double biasZ = ba.getElementAtIndex(2);
23418 
23419         final Matrix ma = generateMaCommonAxis();
23420         final double sx = ma.getElementAt(0, 0);
23421         final double sy = ma.getElementAt(1, 1);
23422         final double sz = ma.getElementAt(2, 2);
23423 
23424         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23425         final double latitude = Math.toRadians(
23426                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23427         final double longitude = Math.toRadians(
23428                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23429         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23430         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23431         final NEDVelocity nedVelocity = new NEDVelocity();
23432         final ECEFPosition ecefPosition = new ECEFPosition();
23433         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23434         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23435                 ecefPosition, ecefVelocity);
23436 
23437         final KnownPositionAccelerometerCalibrator calibrator =
23438                 new KnownPositionAccelerometerCalibrator(nedPosition,
23439                         true, biasX, biasY, biasZ, sx, sy, sz);
23440 
23441         // check default values
23442         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23443         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23444         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23445         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23446         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23447         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23448         final Acceleration bx2 = new Acceleration(0.0,
23449                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23450         calibrator.getInitialBiasXAsAcceleration(bx2);
23451         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23452         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23453         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23454         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23455         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23456         final Acceleration by2 = new Acceleration(0.0,
23457                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23458         calibrator.getInitialBiasYAsAcceleration(by2);
23459         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23460         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23461         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23462         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23463         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23464         final Acceleration bz2 = new Acceleration(0.0,
23465                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23466         calibrator.getInitialBiasZAsAcceleration(bz2);
23467         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23468         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23469         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23470         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23471         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23472         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23473         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23474         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23475         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23476         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23477         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23478         final double[] bias1 = calibrator.getInitialBias();
23479         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23480         final double[] bias2 = new double[3];
23481         calibrator.getInitialBias(bias2);
23482         assertArrayEquals(bias1, bias2, 0.0);
23483         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23484         assertEquals(b1, ba);
23485         final Matrix b2 = new Matrix(3, 1);
23486         calibrator.getInitialBiasAsMatrix(b2);
23487         assertEquals(b1, b2);
23488         final Matrix ma1 = calibrator.getInitialMa();
23489         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23490         final Matrix ma2 = new Matrix(3, 3);
23491         calibrator.getInitialMa(ma2);
23492         assertEquals(ma1, ma2);
23493         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23494         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23495         final NEDPosition nedPosition1 = new NEDPosition();
23496         assertTrue(calibrator.getNedPosition(nedPosition1));
23497         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23498         assertNull(calibrator.getMeasurements());
23499         assertTrue(calibrator.isCommonAxisUsed());
23500         assertNull(calibrator.getListener());
23501         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23502         assertFalse(calibrator.isReady());
23503         assertFalse(calibrator.isRunning());
23504         assertNull(calibrator.getEstimatedBiases());
23505         assertFalse(calibrator.getEstimatedBiases(null));
23506         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23507         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23508         assertNull(calibrator.getEstimatedBiasFx());
23509         assertNull(calibrator.getEstimatedBiasFy());
23510         assertNull(calibrator.getEstimatedBiasFz());
23511         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23512         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23513         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23514         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23515         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23516         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23517         assertNull(calibrator.getEstimatedMa());
23518         assertNull(calibrator.getEstimatedSx());
23519         assertNull(calibrator.getEstimatedSy());
23520         assertNull(calibrator.getEstimatedSz());
23521         assertNull(calibrator.getEstimatedMxy());
23522         assertNull(calibrator.getEstimatedMxz());
23523         assertNull(calibrator.getEstimatedMyx());
23524         assertNull(calibrator.getEstimatedMyz());
23525         assertNull(calibrator.getEstimatedMzx());
23526         assertNull(calibrator.getEstimatedMzy());
23527         assertNull(calibrator.getEstimatedCovariance());
23528         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23529         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23530                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23531         assertNotNull(calibrator.getGroundTruthGravityNorm());
23532         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23533         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23534         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23535                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23536         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23537         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23538         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23539     }
23540 
23541     @Test
23542     public void testConstructor185() throws WrongSizeException {
23543         final Matrix ba = generateBa();
23544         final double biasX = ba.getElementAtIndex(0);
23545         final double biasY = ba.getElementAtIndex(1);
23546         final double biasZ = ba.getElementAtIndex(2);
23547 
23548         final Matrix ma = generateMaCommonAxis();
23549         final double sx = ma.getElementAt(0, 0);
23550         final double sy = ma.getElementAt(1, 1);
23551         final double sz = ma.getElementAt(2, 2);
23552 
23553         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23554         final double latitude = Math.toRadians(
23555                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23556         final double longitude = Math.toRadians(
23557                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23558         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23559         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23560         final NEDVelocity nedVelocity = new NEDVelocity();
23561         final ECEFPosition ecefPosition = new ECEFPosition();
23562         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23563         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23564                 ecefPosition, ecefVelocity);
23565 
23566         final KnownPositionAccelerometerCalibrator calibrator =
23567                 new KnownPositionAccelerometerCalibrator(nedPosition,
23568                         true, biasX, biasY, biasZ, sx, sy, sz,
23569                         this);
23570 
23571         // check default values
23572         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23573         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23574         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23575         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23576         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23577         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23578         final Acceleration bx2 = new Acceleration(0.0,
23579                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23580         calibrator.getInitialBiasXAsAcceleration(bx2);
23581         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23582         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23583         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23584         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23585         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23586         final Acceleration by2 = new Acceleration(0.0,
23587                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23588         calibrator.getInitialBiasYAsAcceleration(by2);
23589         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23590         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23591         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23592         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23593         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23594         final Acceleration bz2 = new Acceleration(0.0,
23595                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23596         calibrator.getInitialBiasZAsAcceleration(bz2);
23597         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23598         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23599         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23600         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23601         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23602         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23603         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23604         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23605         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23606         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23607         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23608         final double[] bias1 = calibrator.getInitialBias();
23609         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23610         final double[] bias2 = new double[3];
23611         calibrator.getInitialBias(bias2);
23612         assertArrayEquals(bias1, bias2, 0.0);
23613         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23614         assertEquals(b1, ba);
23615         final Matrix b2 = new Matrix(3, 1);
23616         calibrator.getInitialBiasAsMatrix(b2);
23617         assertEquals(b1, b2);
23618         final Matrix ma1 = calibrator.getInitialMa();
23619         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23620         final Matrix ma2 = new Matrix(3, 3);
23621         calibrator.getInitialMa(ma2);
23622         assertEquals(ma1, ma2);
23623         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23624         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23625         final NEDPosition nedPosition1 = new NEDPosition();
23626         assertTrue(calibrator.getNedPosition(nedPosition1));
23627         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23628         assertNull(calibrator.getMeasurements());
23629         assertTrue(calibrator.isCommonAxisUsed());
23630         assertSame(calibrator.getListener(), this);
23631         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23632         assertFalse(calibrator.isReady());
23633         assertFalse(calibrator.isRunning());
23634         assertNull(calibrator.getEstimatedBiases());
23635         assertFalse(calibrator.getEstimatedBiases(null));
23636         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23637         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23638         assertNull(calibrator.getEstimatedBiasFx());
23639         assertNull(calibrator.getEstimatedBiasFy());
23640         assertNull(calibrator.getEstimatedBiasFz());
23641         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23642         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23643         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23644         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23645         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23646         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23647         assertNull(calibrator.getEstimatedMa());
23648         assertNull(calibrator.getEstimatedSx());
23649         assertNull(calibrator.getEstimatedSy());
23650         assertNull(calibrator.getEstimatedSz());
23651         assertNull(calibrator.getEstimatedMxy());
23652         assertNull(calibrator.getEstimatedMxz());
23653         assertNull(calibrator.getEstimatedMyx());
23654         assertNull(calibrator.getEstimatedMyz());
23655         assertNull(calibrator.getEstimatedMzx());
23656         assertNull(calibrator.getEstimatedMzy());
23657         assertNull(calibrator.getEstimatedCovariance());
23658         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23659         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23660                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23661         assertNotNull(calibrator.getGroundTruthGravityNorm());
23662         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23663         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23664         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23665                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23666         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23667         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23668         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23669     }
23670 
23671     @Test
23672     public void testConstructor186() throws WrongSizeException {
23673         final Collection<StandardDeviationBodyKinematics> measurements =
23674                 Collections.emptyList();
23675 
23676         final Matrix ba = generateBa();
23677         final double biasX = ba.getElementAtIndex(0);
23678         final double biasY = ba.getElementAtIndex(1);
23679         final double biasZ = ba.getElementAtIndex(2);
23680 
23681         final Matrix ma = generateMaCommonAxis();
23682         final double sx = ma.getElementAt(0, 0);
23683         final double sy = ma.getElementAt(1, 1);
23684         final double sz = ma.getElementAt(2, 2);
23685 
23686         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23687         final double latitude = Math.toRadians(
23688                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23689         final double longitude = Math.toRadians(
23690                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23691         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23692         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23693         final NEDVelocity nedVelocity = new NEDVelocity();
23694         final ECEFPosition ecefPosition = new ECEFPosition();
23695         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23696         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23697                 ecefPosition, ecefVelocity);
23698 
23699         final KnownPositionAccelerometerCalibrator calibrator =
23700                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23701                         true, biasX, biasY, biasZ, sx, sy, sz);
23702 
23703         // check default values
23704         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23705         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23706         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23707         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23708         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23709         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23710         final Acceleration bx2 = new Acceleration(0.0,
23711                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23712         calibrator.getInitialBiasXAsAcceleration(bx2);
23713         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23714         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23715         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23716         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23717         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23718         final Acceleration by2 = new Acceleration(0.0,
23719                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23720         calibrator.getInitialBiasYAsAcceleration(by2);
23721         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23722         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23723         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23724         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23725         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23726         final Acceleration bz2 = new Acceleration(0.0,
23727                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23728         calibrator.getInitialBiasZAsAcceleration(bz2);
23729         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23730         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23731         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23732         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23733         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23734         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23735         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23736         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23737         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23738         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23739         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23740         final double[] bias1 = calibrator.getInitialBias();
23741         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23742         final double[] bias2 = new double[3];
23743         calibrator.getInitialBias(bias2);
23744         assertArrayEquals(bias1, bias2, 0.0);
23745         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23746         assertEquals(b1, ba);
23747         final Matrix b2 = new Matrix(3, 1);
23748         calibrator.getInitialBiasAsMatrix(b2);
23749         assertEquals(b1, b2);
23750         final Matrix ma1 = calibrator.getInitialMa();
23751         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23752         final Matrix ma2 = new Matrix(3, 3);
23753         calibrator.getInitialMa(ma2);
23754         assertEquals(ma1, ma2);
23755         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23756         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23757         final NEDPosition nedPosition1 = new NEDPosition();
23758         assertTrue(calibrator.getNedPosition(nedPosition1));
23759         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23760         assertSame(calibrator.getMeasurements(), measurements);
23761         assertTrue(calibrator.isCommonAxisUsed());
23762         assertNull(calibrator.getListener());
23763         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23764         assertFalse(calibrator.isReady());
23765         assertFalse(calibrator.isRunning());
23766         assertNull(calibrator.getEstimatedBiases());
23767         assertFalse(calibrator.getEstimatedBiases(null));
23768         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23769         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23770         assertNull(calibrator.getEstimatedBiasFx());
23771         assertNull(calibrator.getEstimatedBiasFy());
23772         assertNull(calibrator.getEstimatedBiasFz());
23773         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23774         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23775         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23776         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23777         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23778         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23779         assertNull(calibrator.getEstimatedMa());
23780         assertNull(calibrator.getEstimatedSx());
23781         assertNull(calibrator.getEstimatedSy());
23782         assertNull(calibrator.getEstimatedSz());
23783         assertNull(calibrator.getEstimatedMxy());
23784         assertNull(calibrator.getEstimatedMxz());
23785         assertNull(calibrator.getEstimatedMyx());
23786         assertNull(calibrator.getEstimatedMyz());
23787         assertNull(calibrator.getEstimatedMzx());
23788         assertNull(calibrator.getEstimatedMzy());
23789         assertNull(calibrator.getEstimatedCovariance());
23790         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23791         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23792                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23793         assertNotNull(calibrator.getGroundTruthGravityNorm());
23794         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23795         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23796         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23797                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23798         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23799         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23800         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23801     }
23802 
23803     @Test
23804     public void testConstructor187() throws WrongSizeException {
23805         final Collection<StandardDeviationBodyKinematics> measurements =
23806                 Collections.emptyList();
23807 
23808         final Matrix ba = generateBa();
23809         final double biasX = ba.getElementAtIndex(0);
23810         final double biasY = ba.getElementAtIndex(1);
23811         final double biasZ = ba.getElementAtIndex(2);
23812 
23813         final Matrix ma = generateMaCommonAxis();
23814         final double sx = ma.getElementAt(0, 0);
23815         final double sy = ma.getElementAt(1, 1);
23816         final double sz = ma.getElementAt(2, 2);
23817 
23818         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23819         final double latitude = Math.toRadians(
23820                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23821         final double longitude = Math.toRadians(
23822                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23823         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23824         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23825         final NEDVelocity nedVelocity = new NEDVelocity();
23826         final ECEFPosition ecefPosition = new ECEFPosition();
23827         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23828         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23829                 ecefPosition, ecefVelocity);
23830 
23831         final KnownPositionAccelerometerCalibrator calibrator =
23832                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23833                         true, biasX, biasY, biasZ, sx, sy, sz,
23834                         this);
23835 
23836         // check default values
23837         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23838         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23839         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23840         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23841         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23842         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23843         final Acceleration bx2 = new Acceleration(0.0,
23844                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23845         calibrator.getInitialBiasXAsAcceleration(bx2);
23846         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23847         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23848         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23849         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23850         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23851         final Acceleration by2 = new Acceleration(0.0,
23852                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23853         calibrator.getInitialBiasYAsAcceleration(by2);
23854         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23855         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23856         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23857         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23858         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23859         final Acceleration bz2 = new Acceleration(0.0,
23860                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23861         calibrator.getInitialBiasZAsAcceleration(bz2);
23862         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23863         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23864         assertEquals(calibrator.getInitialSx(), sx, 0.0);
23865         assertEquals(calibrator.getInitialSy(), sy, 0.0);
23866         assertEquals(calibrator.getInitialSz(), sz, 0.0);
23867         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23868         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23869         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23870         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23871         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23872         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23873         final double[] bias1 = calibrator.getInitialBias();
23874         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23875         final double[] bias2 = new double[3];
23876         calibrator.getInitialBias(bias2);
23877         assertArrayEquals(bias1, bias2, 0.0);
23878         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23879         assertEquals(b1, ba);
23880         final Matrix b2 = new Matrix(3, 1);
23881         calibrator.getInitialBiasAsMatrix(b2);
23882         assertEquals(b1, b2);
23883         final Matrix ma1 = calibrator.getInitialMa();
23884         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23885         final Matrix ma2 = new Matrix(3, 3);
23886         calibrator.getInitialMa(ma2);
23887         assertEquals(ma1, ma2);
23888         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23889         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23890         final NEDPosition nedPosition1 = new NEDPosition();
23891         assertTrue(calibrator.getNedPosition(nedPosition1));
23892         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23893         assertSame(calibrator.getMeasurements(), measurements);
23894         assertTrue(calibrator.isCommonAxisUsed());
23895         assertSame(calibrator.getListener(), this);
23896         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23897         assertFalse(calibrator.isReady());
23898         assertFalse(calibrator.isRunning());
23899         assertNull(calibrator.getEstimatedBiases());
23900         assertFalse(calibrator.getEstimatedBiases(null));
23901         assertNull(calibrator.getEstimatedBiasesAsMatrix());
23902         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23903         assertNull(calibrator.getEstimatedBiasFx());
23904         assertNull(calibrator.getEstimatedBiasFy());
23905         assertNull(calibrator.getEstimatedBiasFz());
23906         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23907         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23908         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23909         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23910         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23911         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23912         assertNull(calibrator.getEstimatedMa());
23913         assertNull(calibrator.getEstimatedSx());
23914         assertNull(calibrator.getEstimatedSy());
23915         assertNull(calibrator.getEstimatedSz());
23916         assertNull(calibrator.getEstimatedMxy());
23917         assertNull(calibrator.getEstimatedMxz());
23918         assertNull(calibrator.getEstimatedMyx());
23919         assertNull(calibrator.getEstimatedMyz());
23920         assertNull(calibrator.getEstimatedMzx());
23921         assertNull(calibrator.getEstimatedMzy());
23922         assertNull(calibrator.getEstimatedCovariance());
23923         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23924         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23925                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23926         assertNotNull(calibrator.getGroundTruthGravityNorm());
23927         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23928         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23929         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23930                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23931         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23932         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23933         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23934     }
23935 
23936     @Test
23937     public void testConstructor188() throws WrongSizeException {
23938         final Matrix ba = generateBa();
23939         final double biasX = ba.getElementAtIndex(0);
23940         final double biasY = ba.getElementAtIndex(1);
23941         final double biasZ = ba.getElementAtIndex(2);
23942 
23943         final Acceleration bx = new Acceleration(biasX,
23944                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23945         final Acceleration by = new Acceleration(biasY,
23946                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23947         final Acceleration bz = new Acceleration(biasZ,
23948                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23949 
23950         final Matrix ma = generateMaCommonAxis();
23951         final double sx = ma.getElementAt(0, 0);
23952         final double sy = ma.getElementAt(1, 1);
23953         final double sz = ma.getElementAt(2, 2);
23954 
23955         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23956         final double latitude = Math.toRadians(
23957                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23958         final double longitude = Math.toRadians(
23959                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23960         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23961         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23962         final NEDVelocity nedVelocity = new NEDVelocity();
23963         final ECEFPosition ecefPosition = new ECEFPosition();
23964         final ECEFVelocity ecefVelocity = new ECEFVelocity();
23965         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23966                 ecefPosition, ecefVelocity);
23967 
23968         final KnownPositionAccelerometerCalibrator calibrator =
23969                 new KnownPositionAccelerometerCalibrator(nedPosition,
23970                         bx, by, bz, sx, sy, sz);
23971 
23972         // check default values
23973         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23974         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23975         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23976         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23977         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23978         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23979         final Acceleration bx2 = new Acceleration(0.0,
23980                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23981         calibrator.getInitialBiasXAsAcceleration(bx2);
23982         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23983         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23984         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23985         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23986         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23987         final Acceleration by2 = new Acceleration(0.0,
23988                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23989         calibrator.getInitialBiasYAsAcceleration(by2);
23990         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23991         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23992         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23993         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23994         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23995         final Acceleration bz2 = new Acceleration(0.0,
23996                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23997         calibrator.getInitialBiasZAsAcceleration(bz2);
23998         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23999         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24000         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24001         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24002         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24003         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24004         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24005         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24006         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24007         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24008         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24009         final double[] bias1 = calibrator.getInitialBias();
24010         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24011         final double[] bias2 = new double[3];
24012         calibrator.getInitialBias(bias2);
24013         assertArrayEquals(bias1, bias2, 0.0);
24014         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24015         assertEquals(b1, ba);
24016         final Matrix b2 = new Matrix(3, 1);
24017         calibrator.getInitialBiasAsMatrix(b2);
24018         assertEquals(b1, b2);
24019         final Matrix ma1 = calibrator.getInitialMa();
24020         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24021         final Matrix ma2 = new Matrix(3, 3);
24022         calibrator.getInitialMa(ma2);
24023         assertEquals(ma1, ma2);
24024         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24025         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24026         final NEDPosition nedPosition1 = new NEDPosition();
24027         assertTrue(calibrator.getNedPosition(nedPosition1));
24028         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24029         assertNull(calibrator.getMeasurements());
24030         assertFalse(calibrator.isCommonAxisUsed());
24031         assertNull(calibrator.getListener());
24032         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24033         assertFalse(calibrator.isReady());
24034         assertFalse(calibrator.isRunning());
24035         assertNull(calibrator.getEstimatedBiases());
24036         assertFalse(calibrator.getEstimatedBiases(null));
24037         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24038         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24039         assertNull(calibrator.getEstimatedBiasFx());
24040         assertNull(calibrator.getEstimatedBiasFy());
24041         assertNull(calibrator.getEstimatedBiasFz());
24042         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24043         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24044         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24045         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24046         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24047         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24048         assertNull(calibrator.getEstimatedMa());
24049         assertNull(calibrator.getEstimatedSx());
24050         assertNull(calibrator.getEstimatedSy());
24051         assertNull(calibrator.getEstimatedSz());
24052         assertNull(calibrator.getEstimatedMxy());
24053         assertNull(calibrator.getEstimatedMxz());
24054         assertNull(calibrator.getEstimatedMyx());
24055         assertNull(calibrator.getEstimatedMyz());
24056         assertNull(calibrator.getEstimatedMzx());
24057         assertNull(calibrator.getEstimatedMzy());
24058         assertNull(calibrator.getEstimatedCovariance());
24059         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24060         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24061                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24062         assertNotNull(calibrator.getGroundTruthGravityNorm());
24063         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24064         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24065         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24066                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24067         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24068         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24069         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24070     }
24071 
24072     @Test
24073     public void testConstructor189() throws WrongSizeException {
24074         final Matrix ba = generateBa();
24075         final double biasX = ba.getElementAtIndex(0);
24076         final double biasY = ba.getElementAtIndex(1);
24077         final double biasZ = ba.getElementAtIndex(2);
24078 
24079         final Acceleration bx = new Acceleration(biasX,
24080                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24081         final Acceleration by = new Acceleration(biasY,
24082                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24083         final Acceleration bz = new Acceleration(biasZ,
24084                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24085 
24086         final Matrix ma = generateMaCommonAxis();
24087         final double sx = ma.getElementAt(0, 0);
24088         final double sy = ma.getElementAt(1, 1);
24089         final double sz = ma.getElementAt(2, 2);
24090 
24091         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24092         final double latitude = Math.toRadians(
24093                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24094         final double longitude = Math.toRadians(
24095                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24096         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24097         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24098         final NEDVelocity nedVelocity = new NEDVelocity();
24099         final ECEFPosition ecefPosition = new ECEFPosition();
24100         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24101         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24102                 ecefPosition, ecefVelocity);
24103 
24104         final KnownPositionAccelerometerCalibrator calibrator =
24105                 new KnownPositionAccelerometerCalibrator(nedPosition,
24106                         bx, by, bz, sx, sy, sz, this);
24107 
24108         // check default values
24109         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24110         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24111         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24112         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24113         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24114         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24115         final Acceleration bx2 = new Acceleration(0.0,
24116                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24117         calibrator.getInitialBiasXAsAcceleration(bx2);
24118         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24119         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24120         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24121         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24122         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24123         final Acceleration by2 = new Acceleration(0.0,
24124                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24125         calibrator.getInitialBiasYAsAcceleration(by2);
24126         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24127         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24128         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24129         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24130         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24131         final Acceleration bz2 = new Acceleration(0.0,
24132                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24133         calibrator.getInitialBiasZAsAcceleration(bz2);
24134         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24135         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24136         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24137         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24138         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24139         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24140         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24141         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24142         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24143         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24144         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24145         final double[] bias1 = calibrator.getInitialBias();
24146         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24147         final double[] bias2 = new double[3];
24148         calibrator.getInitialBias(bias2);
24149         assertArrayEquals(bias1, bias2, 0.0);
24150         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24151         assertEquals(b1, ba);
24152         final Matrix b2 = new Matrix(3, 1);
24153         calibrator.getInitialBiasAsMatrix(b2);
24154         assertEquals(b1, b2);
24155         final Matrix ma1 = calibrator.getInitialMa();
24156         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24157         final Matrix ma2 = new Matrix(3, 3);
24158         calibrator.getInitialMa(ma2);
24159         assertEquals(ma1, ma2);
24160         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24161         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24162         final NEDPosition nedPosition1 = new NEDPosition();
24163         assertTrue(calibrator.getNedPosition(nedPosition1));
24164         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24165         assertNull(calibrator.getMeasurements());
24166         assertFalse(calibrator.isCommonAxisUsed());
24167         assertSame(calibrator.getListener(), this);
24168         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24169         assertFalse(calibrator.isReady());
24170         assertFalse(calibrator.isRunning());
24171         assertNull(calibrator.getEstimatedBiases());
24172         assertFalse(calibrator.getEstimatedBiases(null));
24173         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24174         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24175         assertNull(calibrator.getEstimatedBiasFx());
24176         assertNull(calibrator.getEstimatedBiasFy());
24177         assertNull(calibrator.getEstimatedBiasFz());
24178         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24179         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24180         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24181         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24182         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24183         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24184         assertNull(calibrator.getEstimatedMa());
24185         assertNull(calibrator.getEstimatedSx());
24186         assertNull(calibrator.getEstimatedSy());
24187         assertNull(calibrator.getEstimatedSz());
24188         assertNull(calibrator.getEstimatedMxy());
24189         assertNull(calibrator.getEstimatedMxz());
24190         assertNull(calibrator.getEstimatedMyx());
24191         assertNull(calibrator.getEstimatedMyz());
24192         assertNull(calibrator.getEstimatedMzx());
24193         assertNull(calibrator.getEstimatedMzy());
24194         assertNull(calibrator.getEstimatedCovariance());
24195         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24196         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24197                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24198         assertNotNull(calibrator.getGroundTruthGravityNorm());
24199         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24200         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24201         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24202                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24203         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24204         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24205         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24206     }
24207 
24208     @Test
24209     public void testConstructor190() throws WrongSizeException {
24210         final Collection<StandardDeviationBodyKinematics> measurements =
24211                 Collections.emptyList();
24212 
24213         final Matrix ba = generateBa();
24214         final double biasX = ba.getElementAtIndex(0);
24215         final double biasY = ba.getElementAtIndex(1);
24216         final double biasZ = ba.getElementAtIndex(2);
24217 
24218         final Acceleration bx = new Acceleration(biasX,
24219                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24220         final Acceleration by = new Acceleration(biasY,
24221                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24222         final Acceleration bz = new Acceleration(biasZ,
24223                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24224 
24225         final Matrix ma = generateMaCommonAxis();
24226         final double sx = ma.getElementAt(0, 0);
24227         final double sy = ma.getElementAt(1, 1);
24228         final double sz = ma.getElementAt(2, 2);
24229 
24230         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24231         final double latitude = Math.toRadians(
24232                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24233         final double longitude = Math.toRadians(
24234                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24235         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24236         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24237         final NEDVelocity nedVelocity = new NEDVelocity();
24238         final ECEFPosition ecefPosition = new ECEFPosition();
24239         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24240         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24241                 ecefPosition, ecefVelocity);
24242 
24243         final KnownPositionAccelerometerCalibrator calibrator =
24244                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24245                         bx, by, bz, sx, sy, sz);
24246 
24247         // check default values
24248         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24249         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24250         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24251         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24252         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24253         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24254         final Acceleration bx2 = new Acceleration(0.0,
24255                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24256         calibrator.getInitialBiasXAsAcceleration(bx2);
24257         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24258         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24259         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24260         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24261         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24262         final Acceleration by2 = new Acceleration(0.0,
24263                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24264         calibrator.getInitialBiasYAsAcceleration(by2);
24265         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24266         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24267         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24268         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24269         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24270         final Acceleration bz2 = new Acceleration(0.0,
24271                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24272         calibrator.getInitialBiasZAsAcceleration(bz2);
24273         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24274         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24275         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24276         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24277         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24278         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24279         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24280         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24281         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24282         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24283         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24284         final double[] bias1 = calibrator.getInitialBias();
24285         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24286         final double[] bias2 = new double[3];
24287         calibrator.getInitialBias(bias2);
24288         assertArrayEquals(bias1, bias2, 0.0);
24289         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24290         assertEquals(b1, ba);
24291         final Matrix b2 = new Matrix(3, 1);
24292         calibrator.getInitialBiasAsMatrix(b2);
24293         assertEquals(b1, b2);
24294         final Matrix ma1 = calibrator.getInitialMa();
24295         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24296         final Matrix ma2 = new Matrix(3, 3);
24297         calibrator.getInitialMa(ma2);
24298         assertEquals(ma1, ma2);
24299         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24300         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24301         final NEDPosition nedPosition1 = new NEDPosition();
24302         assertTrue(calibrator.getNedPosition(nedPosition1));
24303         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24304         assertSame(calibrator.getMeasurements(), measurements);
24305         assertFalse(calibrator.isCommonAxisUsed());
24306         assertNull(calibrator.getListener());
24307         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24308         assertFalse(calibrator.isReady());
24309         assertFalse(calibrator.isRunning());
24310         assertNull(calibrator.getEstimatedBiases());
24311         assertFalse(calibrator.getEstimatedBiases(null));
24312         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24313         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24314         assertNull(calibrator.getEstimatedBiasFx());
24315         assertNull(calibrator.getEstimatedBiasFy());
24316         assertNull(calibrator.getEstimatedBiasFz());
24317         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24318         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24319         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24320         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24321         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24322         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24323         assertNull(calibrator.getEstimatedMa());
24324         assertNull(calibrator.getEstimatedSx());
24325         assertNull(calibrator.getEstimatedSy());
24326         assertNull(calibrator.getEstimatedSz());
24327         assertNull(calibrator.getEstimatedMxy());
24328         assertNull(calibrator.getEstimatedMxz());
24329         assertNull(calibrator.getEstimatedMyx());
24330         assertNull(calibrator.getEstimatedMyz());
24331         assertNull(calibrator.getEstimatedMzx());
24332         assertNull(calibrator.getEstimatedMzy());
24333         assertNull(calibrator.getEstimatedCovariance());
24334         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24335         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24336                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24337         assertNotNull(calibrator.getGroundTruthGravityNorm());
24338         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24339         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24340         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24341                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24342         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24343         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24344         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24345     }
24346 
24347     @Test
24348     public void testConstructor191() throws WrongSizeException {
24349         final Collection<StandardDeviationBodyKinematics> measurements =
24350                 Collections.emptyList();
24351 
24352         final Matrix ba = generateBa();
24353         final double biasX = ba.getElementAtIndex(0);
24354         final double biasY = ba.getElementAtIndex(1);
24355         final double biasZ = ba.getElementAtIndex(2);
24356 
24357         final Acceleration bx = new Acceleration(biasX,
24358                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24359         final Acceleration by = new Acceleration(biasY,
24360                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24361         final Acceleration bz = new Acceleration(biasZ,
24362                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24363 
24364         final Matrix ma = generateMaCommonAxis();
24365         final double sx = ma.getElementAt(0, 0);
24366         final double sy = ma.getElementAt(1, 1);
24367         final double sz = ma.getElementAt(2, 2);
24368 
24369         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24370         final double latitude = Math.toRadians(
24371                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24372         final double longitude = Math.toRadians(
24373                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24374         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24375         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24376         final NEDVelocity nedVelocity = new NEDVelocity();
24377         final ECEFPosition ecefPosition = new ECEFPosition();
24378         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24379         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24380                 ecefPosition, ecefVelocity);
24381 
24382         final KnownPositionAccelerometerCalibrator calibrator =
24383                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24384                         bx, by, bz, sx, sy, sz, this);
24385 
24386         // check default values
24387         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24388         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24389         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24390         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24391         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24392         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24393         final Acceleration bx2 = new Acceleration(0.0,
24394                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24395         calibrator.getInitialBiasXAsAcceleration(bx2);
24396         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24397         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24398         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24399         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24400         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24401         final Acceleration by2 = new Acceleration(0.0,
24402                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24403         calibrator.getInitialBiasYAsAcceleration(by2);
24404         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24405         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24406         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24407         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24408         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24409         final Acceleration bz2 = new Acceleration(0.0,
24410                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24411         calibrator.getInitialBiasZAsAcceleration(bz2);
24412         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24413         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24414         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24415         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24416         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24417         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24418         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24419         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24420         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24421         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24422         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24423         final double[] bias1 = calibrator.getInitialBias();
24424         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24425         final double[] bias2 = new double[3];
24426         calibrator.getInitialBias(bias2);
24427         assertArrayEquals(bias1, bias2, 0.0);
24428         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24429         assertEquals(b1, ba);
24430         final Matrix b2 = new Matrix(3, 1);
24431         calibrator.getInitialBiasAsMatrix(b2);
24432         assertEquals(b1, b2);
24433         final Matrix ma1 = calibrator.getInitialMa();
24434         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24435         final Matrix ma2 = new Matrix(3, 3);
24436         calibrator.getInitialMa(ma2);
24437         assertEquals(ma1, ma2);
24438         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24439         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24440         final NEDPosition nedPosition1 = new NEDPosition();
24441         assertTrue(calibrator.getNedPosition(nedPosition1));
24442         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24443         assertSame(calibrator.getMeasurements(), measurements);
24444         assertFalse(calibrator.isCommonAxisUsed());
24445         assertSame(calibrator.getListener(), this);
24446         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24447         assertFalse(calibrator.isReady());
24448         assertFalse(calibrator.isRunning());
24449         assertNull(calibrator.getEstimatedBiases());
24450         assertFalse(calibrator.getEstimatedBiases(null));
24451         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24452         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24453         assertNull(calibrator.getEstimatedBiasFx());
24454         assertNull(calibrator.getEstimatedBiasFy());
24455         assertNull(calibrator.getEstimatedBiasFz());
24456         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24457         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24458         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24459         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24460         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24461         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24462         assertNull(calibrator.getEstimatedMa());
24463         assertNull(calibrator.getEstimatedSx());
24464         assertNull(calibrator.getEstimatedSy());
24465         assertNull(calibrator.getEstimatedSz());
24466         assertNull(calibrator.getEstimatedMxy());
24467         assertNull(calibrator.getEstimatedMxz());
24468         assertNull(calibrator.getEstimatedMyx());
24469         assertNull(calibrator.getEstimatedMyz());
24470         assertNull(calibrator.getEstimatedMzx());
24471         assertNull(calibrator.getEstimatedMzy());
24472         assertNull(calibrator.getEstimatedCovariance());
24473         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24474         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24475                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24476         assertNotNull(calibrator.getGroundTruthGravityNorm());
24477         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24478         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24479         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24480                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24481         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24482         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24483         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24484     }
24485 
24486     @Test
24487     public void testConstructor192() throws WrongSizeException {
24488         final Matrix ba = generateBa();
24489         final double biasX = ba.getElementAtIndex(0);
24490         final double biasY = ba.getElementAtIndex(1);
24491         final double biasZ = ba.getElementAtIndex(2);
24492 
24493         final Acceleration bx = new Acceleration(biasX,
24494                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24495         final Acceleration by = new Acceleration(biasY,
24496                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24497         final Acceleration bz = new Acceleration(biasZ,
24498                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24499 
24500         final Matrix ma = generateMaCommonAxis();
24501         final double sx = ma.getElementAt(0, 0);
24502         final double sy = ma.getElementAt(1, 1);
24503         final double sz = ma.getElementAt(2, 2);
24504 
24505         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24506         final double latitude = Math.toRadians(
24507                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24508         final double longitude = Math.toRadians(
24509                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24510         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24511         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24512         final NEDVelocity nedVelocity = new NEDVelocity();
24513         final ECEFPosition ecefPosition = new ECEFPosition();
24514         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24515         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24516                 ecefPosition, ecefVelocity);
24517 
24518         final KnownPositionAccelerometerCalibrator calibrator =
24519                 new KnownPositionAccelerometerCalibrator(nedPosition,
24520                         true, bx, by, bz, sx, sy, sz);
24521 
24522         // check default values
24523         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24524         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24525         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24526         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24527         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24528         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24529         final Acceleration bx2 = new Acceleration(0.0,
24530                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24531         calibrator.getInitialBiasXAsAcceleration(bx2);
24532         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24533         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24534         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24535         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24536         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24537         final Acceleration by2 = new Acceleration(0.0,
24538                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24539         calibrator.getInitialBiasYAsAcceleration(by2);
24540         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24541         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24542         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24543         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24544         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24545         final Acceleration bz2 = new Acceleration(0.0,
24546                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24547         calibrator.getInitialBiasZAsAcceleration(bz2);
24548         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24549         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24550         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24551         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24552         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24553         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24554         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24555         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24556         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24557         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24558         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24559         final double[] bias1 = calibrator.getInitialBias();
24560         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24561         final double[] bias2 = new double[3];
24562         calibrator.getInitialBias(bias2);
24563         assertArrayEquals(bias1, bias2, 0.0);
24564         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24565         assertEquals(b1, ba);
24566         final Matrix b2 = new Matrix(3, 1);
24567         calibrator.getInitialBiasAsMatrix(b2);
24568         assertEquals(b1, b2);
24569         final Matrix ma1 = calibrator.getInitialMa();
24570         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24571         final Matrix ma2 = new Matrix(3, 3);
24572         calibrator.getInitialMa(ma2);
24573         assertEquals(ma1, ma2);
24574         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24575         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24576         final NEDPosition nedPosition1 = new NEDPosition();
24577         assertTrue(calibrator.getNedPosition(nedPosition1));
24578         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24579         assertNull(calibrator.getMeasurements());
24580         assertTrue(calibrator.isCommonAxisUsed());
24581         assertNull(calibrator.getListener());
24582         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24583         assertFalse(calibrator.isReady());
24584         assertFalse(calibrator.isRunning());
24585         assertNull(calibrator.getEstimatedBiases());
24586         assertFalse(calibrator.getEstimatedBiases(null));
24587         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24588         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24589         assertNull(calibrator.getEstimatedBiasFx());
24590         assertNull(calibrator.getEstimatedBiasFy());
24591         assertNull(calibrator.getEstimatedBiasFz());
24592         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24593         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24594         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24595         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24596         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24597         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24598         assertNull(calibrator.getEstimatedMa());
24599         assertNull(calibrator.getEstimatedSx());
24600         assertNull(calibrator.getEstimatedSy());
24601         assertNull(calibrator.getEstimatedSz());
24602         assertNull(calibrator.getEstimatedMxy());
24603         assertNull(calibrator.getEstimatedMxz());
24604         assertNull(calibrator.getEstimatedMyx());
24605         assertNull(calibrator.getEstimatedMyz());
24606         assertNull(calibrator.getEstimatedMzx());
24607         assertNull(calibrator.getEstimatedMzy());
24608         assertNull(calibrator.getEstimatedCovariance());
24609         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24610         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24611                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24612         assertNotNull(calibrator.getGroundTruthGravityNorm());
24613         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24614         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24615         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24616                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24617         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24618         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24619         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24620     }
24621 
24622     @Test
24623     public void testConstructor193() throws WrongSizeException {
24624         final Matrix ba = generateBa();
24625         final double biasX = ba.getElementAtIndex(0);
24626         final double biasY = ba.getElementAtIndex(1);
24627         final double biasZ = ba.getElementAtIndex(2);
24628 
24629         final Acceleration bx = new Acceleration(biasX,
24630                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24631         final Acceleration by = new Acceleration(biasY,
24632                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24633         final Acceleration bz = new Acceleration(biasZ,
24634                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24635 
24636         final Matrix ma = generateMaCommonAxis();
24637         final double sx = ma.getElementAt(0, 0);
24638         final double sy = ma.getElementAt(1, 1);
24639         final double sz = ma.getElementAt(2, 2);
24640 
24641         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24642         final double latitude = Math.toRadians(
24643                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24644         final double longitude = Math.toRadians(
24645                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24646         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24647         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24648         final NEDVelocity nedVelocity = new NEDVelocity();
24649         final ECEFPosition ecefPosition = new ECEFPosition();
24650         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24651         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24652                 ecefPosition, ecefVelocity);
24653 
24654         final KnownPositionAccelerometerCalibrator calibrator =
24655                 new KnownPositionAccelerometerCalibrator(nedPosition,
24656                         true, bx, by, bz, sx, sy, sz, this);
24657 
24658         // check default values
24659         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24660         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24661         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24662         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24663         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24664         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24665         final Acceleration bx2 = new Acceleration(0.0,
24666                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24667         calibrator.getInitialBiasXAsAcceleration(bx2);
24668         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24669         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24670         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24671         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24672         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24673         final Acceleration by2 = new Acceleration(0.0,
24674                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24675         calibrator.getInitialBiasYAsAcceleration(by2);
24676         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24677         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24678         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24679         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24680         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24681         final Acceleration bz2 = new Acceleration(0.0,
24682                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24683         calibrator.getInitialBiasZAsAcceleration(bz2);
24684         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24685         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24686         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24687         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24688         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24689         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24690         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24691         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24692         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24693         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24694         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24695         final double[] bias1 = calibrator.getInitialBias();
24696         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24697         final double[] bias2 = new double[3];
24698         calibrator.getInitialBias(bias2);
24699         assertArrayEquals(bias1, bias2, 0.0);
24700         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24701         assertEquals(b1, ba);
24702         final Matrix b2 = new Matrix(3, 1);
24703         calibrator.getInitialBiasAsMatrix(b2);
24704         assertEquals(b1, b2);
24705         final Matrix ma1 = calibrator.getInitialMa();
24706         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24707         final Matrix ma2 = new Matrix(3, 3);
24708         calibrator.getInitialMa(ma2);
24709         assertEquals(ma1, ma2);
24710         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24711         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24712         final NEDPosition nedPosition1 = new NEDPosition();
24713         assertTrue(calibrator.getNedPosition(nedPosition1));
24714         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24715         assertNull(calibrator.getMeasurements());
24716         assertTrue(calibrator.isCommonAxisUsed());
24717         assertSame(calibrator.getListener(), this);
24718         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24719         assertFalse(calibrator.isReady());
24720         assertFalse(calibrator.isRunning());
24721         assertNull(calibrator.getEstimatedBiases());
24722         assertFalse(calibrator.getEstimatedBiases(null));
24723         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24724         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24725         assertNull(calibrator.getEstimatedBiasFx());
24726         assertNull(calibrator.getEstimatedBiasFy());
24727         assertNull(calibrator.getEstimatedBiasFz());
24728         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24729         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24730         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24731         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24732         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24733         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24734         assertNull(calibrator.getEstimatedMa());
24735         assertNull(calibrator.getEstimatedSx());
24736         assertNull(calibrator.getEstimatedSy());
24737         assertNull(calibrator.getEstimatedSz());
24738         assertNull(calibrator.getEstimatedMxy());
24739         assertNull(calibrator.getEstimatedMxz());
24740         assertNull(calibrator.getEstimatedMyx());
24741         assertNull(calibrator.getEstimatedMyz());
24742         assertNull(calibrator.getEstimatedMzx());
24743         assertNull(calibrator.getEstimatedMzy());
24744         assertNull(calibrator.getEstimatedCovariance());
24745         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24746         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24747                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24748         assertNotNull(calibrator.getGroundTruthGravityNorm());
24749         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24750         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24751         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24752                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24753         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24754         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24755         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24756     }
24757 
24758     @Test
24759     public void testConstructor194() throws WrongSizeException {
24760         final Collection<StandardDeviationBodyKinematics> measurements =
24761                 Collections.emptyList();
24762 
24763         final Matrix ba = generateBa();
24764         final double biasX = ba.getElementAtIndex(0);
24765         final double biasY = ba.getElementAtIndex(1);
24766         final double biasZ = ba.getElementAtIndex(2);
24767 
24768         final Acceleration bx = new Acceleration(biasX,
24769                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24770         final Acceleration by = new Acceleration(biasY,
24771                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24772         final Acceleration bz = new Acceleration(biasZ,
24773                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24774 
24775         final Matrix ma = generateMaCommonAxis();
24776         final double sx = ma.getElementAt(0, 0);
24777         final double sy = ma.getElementAt(1, 1);
24778         final double sz = ma.getElementAt(2, 2);
24779 
24780         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24781         final double latitude = Math.toRadians(
24782                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24783         final double longitude = Math.toRadians(
24784                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24785         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24786         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24787         final NEDVelocity nedVelocity = new NEDVelocity();
24788         final ECEFPosition ecefPosition = new ECEFPosition();
24789         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24790         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24791                 ecefPosition, ecefVelocity);
24792 
24793         final KnownPositionAccelerometerCalibrator calibrator =
24794                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24795                         true, bx, by, bz, sx, sy, sz);
24796 
24797         // check default values
24798         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24799         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24800         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24801         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24802         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24803         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24804         final Acceleration bx2 = new Acceleration(0.0,
24805                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24806         calibrator.getInitialBiasXAsAcceleration(bx2);
24807         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24808         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24809         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24810         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24811         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24812         final Acceleration by2 = new Acceleration(0.0,
24813                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24814         calibrator.getInitialBiasYAsAcceleration(by2);
24815         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24816         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24817         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24818         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24819         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24820         final Acceleration bz2 = new Acceleration(0.0,
24821                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24822         calibrator.getInitialBiasZAsAcceleration(bz2);
24823         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24824         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24825         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24826         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24827         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24828         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24829         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24830         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24831         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24832         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24833         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24834         final double[] bias1 = calibrator.getInitialBias();
24835         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24836         final double[] bias2 = new double[3];
24837         calibrator.getInitialBias(bias2);
24838         assertArrayEquals(bias1, bias2, 0.0);
24839         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24840         assertEquals(b1, ba);
24841         final Matrix b2 = new Matrix(3, 1);
24842         calibrator.getInitialBiasAsMatrix(b2);
24843         assertEquals(b1, b2);
24844         final Matrix ma1 = calibrator.getInitialMa();
24845         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24846         final Matrix ma2 = new Matrix(3, 3);
24847         calibrator.getInitialMa(ma2);
24848         assertEquals(ma1, ma2);
24849         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24850         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24851         final NEDPosition nedPosition1 = new NEDPosition();
24852         assertTrue(calibrator.getNedPosition(nedPosition1));
24853         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24854         assertSame(calibrator.getMeasurements(), measurements);
24855         assertTrue(calibrator.isCommonAxisUsed());
24856         assertNull(calibrator.getListener());
24857         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24858         assertFalse(calibrator.isReady());
24859         assertFalse(calibrator.isRunning());
24860         assertNull(calibrator.getEstimatedBiases());
24861         assertFalse(calibrator.getEstimatedBiases(null));
24862         assertNull(calibrator.getEstimatedBiasesAsMatrix());
24863         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24864         assertNull(calibrator.getEstimatedBiasFx());
24865         assertNull(calibrator.getEstimatedBiasFy());
24866         assertNull(calibrator.getEstimatedBiasFz());
24867         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24868         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24869         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24870         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24871         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24872         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24873         assertNull(calibrator.getEstimatedMa());
24874         assertNull(calibrator.getEstimatedSx());
24875         assertNull(calibrator.getEstimatedSy());
24876         assertNull(calibrator.getEstimatedSz());
24877         assertNull(calibrator.getEstimatedMxy());
24878         assertNull(calibrator.getEstimatedMxz());
24879         assertNull(calibrator.getEstimatedMyx());
24880         assertNull(calibrator.getEstimatedMyz());
24881         assertNull(calibrator.getEstimatedMzx());
24882         assertNull(calibrator.getEstimatedMzy());
24883         assertNull(calibrator.getEstimatedCovariance());
24884         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24885         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24886                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24887         assertNotNull(calibrator.getGroundTruthGravityNorm());
24888         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24889         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24890         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24891                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24892         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24893         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24894         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24895     }
24896 
24897     @Test
24898     public void testConstructor195() throws WrongSizeException {
24899         final Collection<StandardDeviationBodyKinematics> measurements =
24900                 Collections.emptyList();
24901 
24902         final Matrix ba = generateBa();
24903         final double biasX = ba.getElementAtIndex(0);
24904         final double biasY = ba.getElementAtIndex(1);
24905         final double biasZ = ba.getElementAtIndex(2);
24906 
24907         final Acceleration bx = new Acceleration(biasX,
24908                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24909         final Acceleration by = new Acceleration(biasY,
24910                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24911         final Acceleration bz = new Acceleration(biasZ,
24912                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24913 
24914         final Matrix ma = generateMaCommonAxis();
24915         final double sx = ma.getElementAt(0, 0);
24916         final double sy = ma.getElementAt(1, 1);
24917         final double sz = ma.getElementAt(2, 2);
24918 
24919         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24920         final double latitude = Math.toRadians(
24921                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24922         final double longitude = Math.toRadians(
24923                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24924         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24925         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24926         final NEDVelocity nedVelocity = new NEDVelocity();
24927         final ECEFPosition ecefPosition = new ECEFPosition();
24928         final ECEFVelocity ecefVelocity = new ECEFVelocity();
24929         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24930                 ecefPosition, ecefVelocity);
24931 
24932         final KnownPositionAccelerometerCalibrator calibrator =
24933                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24934                         true, bx, by, bz, sx, sy, sz, this);
24935 
24936         // check default values
24937         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24938         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24939         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24940         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24941         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24942         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24943         final Acceleration bx2 = new Acceleration(0.0,
24944                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24945         calibrator.getInitialBiasXAsAcceleration(bx2);
24946         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24947         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24948         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24949         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24950         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24951         final Acceleration by2 = new Acceleration(0.0,
24952                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24953         calibrator.getInitialBiasYAsAcceleration(by2);
24954         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24955         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24956         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24957         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24958         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24959         final Acceleration bz2 = new Acceleration(0.0,
24960                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24961         calibrator.getInitialBiasZAsAcceleration(bz2);
24962         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24963         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24964         assertEquals(calibrator.getInitialSx(), sx, 0.0);
24965         assertEquals(calibrator.getInitialSy(), sy, 0.0);
24966         assertEquals(calibrator.getInitialSz(), sz, 0.0);
24967         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24968         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24969         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24970         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24971         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24972         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24973         final double[] bias1 = calibrator.getInitialBias();
24974         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24975         final double[] bias2 = new double[3];
24976         calibrator.getInitialBias(bias2);
24977         assertArrayEquals(bias1, bias2, 0.0);
24978         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24979         assertEquals(b1, ba);
24980         final Matrix b2 = new Matrix(3, 1);
24981         calibrator.getInitialBiasAsMatrix(b2);
24982         assertEquals(b1, b2);
24983         final Matrix ma1 = calibrator.getInitialMa();
24984         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24985         final Matrix ma2 = new Matrix(3, 3);
24986         calibrator.getInitialMa(ma2);
24987         assertEquals(ma1, ma2);
24988         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24989         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24990         final NEDPosition nedPosition1 = new NEDPosition();
24991         assertTrue(calibrator.getNedPosition(nedPosition1));
24992         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24993         assertSame(calibrator.getMeasurements(), measurements);
24994         assertTrue(calibrator.isCommonAxisUsed());
24995         assertSame(calibrator.getListener(), this);
24996         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24997         assertFalse(calibrator.isReady());
24998         assertFalse(calibrator.isRunning());
24999         assertNull(calibrator.getEstimatedBiases());
25000         assertFalse(calibrator.getEstimatedBiases(null));
25001         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25002         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25003         assertNull(calibrator.getEstimatedBiasFx());
25004         assertNull(calibrator.getEstimatedBiasFy());
25005         assertNull(calibrator.getEstimatedBiasFz());
25006         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25007         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25008         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25009         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25010         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25011         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25012         assertNull(calibrator.getEstimatedMa());
25013         assertNull(calibrator.getEstimatedSx());
25014         assertNull(calibrator.getEstimatedSy());
25015         assertNull(calibrator.getEstimatedSz());
25016         assertNull(calibrator.getEstimatedMxy());
25017         assertNull(calibrator.getEstimatedMxz());
25018         assertNull(calibrator.getEstimatedMyx());
25019         assertNull(calibrator.getEstimatedMyz());
25020         assertNull(calibrator.getEstimatedMzx());
25021         assertNull(calibrator.getEstimatedMzy());
25022         assertNull(calibrator.getEstimatedCovariance());
25023         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25024         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25025                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25026         assertNotNull(calibrator.getGroundTruthGravityNorm());
25027         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25028         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25029         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25030                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25031         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25032         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25033         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25034     }
25035 
25036     @Test
25037     public void testConstructor196() throws WrongSizeException {
25038         final Matrix ba = generateBa();
25039         final double biasX = ba.getElementAtIndex(0);
25040         final double biasY = ba.getElementAtIndex(1);
25041         final double biasZ = ba.getElementAtIndex(2);
25042 
25043         final Matrix ma = generateMaCommonAxis();
25044         final double sx = ma.getElementAt(0, 0);
25045         final double sy = ma.getElementAt(1, 1);
25046         final double sz = ma.getElementAt(2, 2);
25047         final double mxy = ma.getElementAt(0, 1);
25048         final double mxz = ma.getElementAt(0, 2);
25049         final double myx = ma.getElementAt(1, 0);
25050         final double myz = ma.getElementAt(1, 2);
25051         final double mzx = ma.getElementAt(2, 0);
25052         final double mzy = ma.getElementAt(2, 1);
25053 
25054         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25055         final double latitude = Math.toRadians(
25056                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25057         final double longitude = Math.toRadians(
25058                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25059         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25060         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25061         final NEDVelocity nedVelocity = new NEDVelocity();
25062         final ECEFPosition ecefPosition = new ECEFPosition();
25063         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25064         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25065                 ecefPosition, ecefVelocity);
25066 
25067         final KnownPositionAccelerometerCalibrator calibrator =
25068                 new KnownPositionAccelerometerCalibrator(nedPosition,
25069                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25070                         myx, myz, mzx, mzy);
25071 
25072         // check default values
25073         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25074         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25075         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25076         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25077         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25078         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25079         final Acceleration bx2 = new Acceleration(0.0,
25080                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25081         calibrator.getInitialBiasXAsAcceleration(bx2);
25082         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25083         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25084         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25085         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25086         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25087         final Acceleration by2 = new Acceleration(0.0,
25088                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25089         calibrator.getInitialBiasYAsAcceleration(by2);
25090         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25091         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25092         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25093         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25094         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25095         final Acceleration bz2 = new Acceleration(0.0,
25096                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25097         calibrator.getInitialBiasZAsAcceleration(bz2);
25098         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25099         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25100         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25101         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25102         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25103         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25104         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25105         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25106         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25107         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25108         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25109         final double[] bias1 = calibrator.getInitialBias();
25110         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25111         final double[] bias2 = new double[3];
25112         calibrator.getInitialBias(bias2);
25113         assertArrayEquals(bias1, bias2, 0.0);
25114         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25115         assertEquals(b1, ba);
25116         final Matrix b2 = new Matrix(3, 1);
25117         calibrator.getInitialBiasAsMatrix(b2);
25118         assertEquals(b1, b2);
25119         final Matrix ma1 = new Matrix(3, 3);
25120         ma1.setSubmatrix(0, 0,
25121                 2, 2,
25122                 new double[]{sx, myx, mzx,
25123                         mxy, sy, mzy,
25124                         mxz, myz, sz});
25125         assertEquals(calibrator.getInitialMa(), ma1);
25126         final Matrix ma2 = new Matrix(3, 3);
25127         calibrator.getInitialMa(ma2);
25128         assertEquals(ma1, ma2);
25129         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25130         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25131         final NEDPosition nedPosition1 = new NEDPosition();
25132         assertTrue(calibrator.getNedPosition(nedPosition1));
25133         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25134         assertNull(calibrator.getMeasurements());
25135         assertFalse(calibrator.isCommonAxisUsed());
25136         assertNull(calibrator.getListener());
25137         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25138         assertFalse(calibrator.isReady());
25139         assertFalse(calibrator.isRunning());
25140         assertNull(calibrator.getEstimatedBiases());
25141         assertFalse(calibrator.getEstimatedBiases(null));
25142         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25143         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25144         assertNull(calibrator.getEstimatedBiasFx());
25145         assertNull(calibrator.getEstimatedBiasFy());
25146         assertNull(calibrator.getEstimatedBiasFz());
25147         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25148         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25149         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25150         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25151         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25152         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25153         assertNull(calibrator.getEstimatedMa());
25154         assertNull(calibrator.getEstimatedSx());
25155         assertNull(calibrator.getEstimatedSy());
25156         assertNull(calibrator.getEstimatedSz());
25157         assertNull(calibrator.getEstimatedMxy());
25158         assertNull(calibrator.getEstimatedMxz());
25159         assertNull(calibrator.getEstimatedMyx());
25160         assertNull(calibrator.getEstimatedMyz());
25161         assertNull(calibrator.getEstimatedMzx());
25162         assertNull(calibrator.getEstimatedMzy());
25163         assertNull(calibrator.getEstimatedCovariance());
25164         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25165         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25166                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25167         assertNotNull(calibrator.getGroundTruthGravityNorm());
25168         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25169         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25170         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25171                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25172         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25173         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25174         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25175     }
25176 
25177     @Test
25178     public void testConstructor197() throws WrongSizeException {
25179         final Collection<StandardDeviationBodyKinematics> measurements =
25180                 Collections.emptyList();
25181 
25182         final Matrix ba = generateBa();
25183         final double biasX = ba.getElementAtIndex(0);
25184         final double biasY = ba.getElementAtIndex(1);
25185         final double biasZ = ba.getElementAtIndex(2);
25186 
25187         final Matrix ma = generateMaCommonAxis();
25188         final double sx = ma.getElementAt(0, 0);
25189         final double sy = ma.getElementAt(1, 1);
25190         final double sz = ma.getElementAt(2, 2);
25191         final double mxy = ma.getElementAt(0, 1);
25192         final double mxz = ma.getElementAt(0, 2);
25193         final double myx = ma.getElementAt(1, 0);
25194         final double myz = ma.getElementAt(1, 2);
25195         final double mzx = ma.getElementAt(2, 0);
25196         final double mzy = ma.getElementAt(2, 1);
25197 
25198         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25199         final double latitude = Math.toRadians(
25200                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25201         final double longitude = Math.toRadians(
25202                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25203         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25204         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25205         final NEDVelocity nedVelocity = new NEDVelocity();
25206         final ECEFPosition ecefPosition = new ECEFPosition();
25207         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25208         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25209                 ecefPosition, ecefVelocity);
25210 
25211         final KnownPositionAccelerometerCalibrator calibrator =
25212                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25213                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25214                         myx, myz, mzx, mzy);
25215 
25216         // check default values
25217         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25218         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25219         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25220         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25221         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25222         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25223         final Acceleration bx2 = new Acceleration(0.0,
25224                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25225         calibrator.getInitialBiasXAsAcceleration(bx2);
25226         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25227         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25228         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25229         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25230         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25231         final Acceleration by2 = new Acceleration(0.0,
25232                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25233         calibrator.getInitialBiasYAsAcceleration(by2);
25234         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25235         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25236         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25237         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25238         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25239         final Acceleration bz2 = new Acceleration(0.0,
25240                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25241         calibrator.getInitialBiasZAsAcceleration(bz2);
25242         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25243         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25244         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25245         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25246         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25247         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25248         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25249         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25250         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25251         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25252         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25253         final double[] bias1 = calibrator.getInitialBias();
25254         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25255         final double[] bias2 = new double[3];
25256         calibrator.getInitialBias(bias2);
25257         assertArrayEquals(bias1, bias2, 0.0);
25258         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25259         assertEquals(b1, ba);
25260         final Matrix b2 = new Matrix(3, 1);
25261         calibrator.getInitialBiasAsMatrix(b2);
25262         assertEquals(b1, b2);
25263         final Matrix ma1 = new Matrix(3, 3);
25264         ma1.setSubmatrix(0, 0,
25265                 2, 2,
25266                 new double[]{sx, myx, mzx,
25267                         mxy, sy, mzy,
25268                         mxz, myz, sz});
25269         assertEquals(calibrator.getInitialMa(), ma1);
25270         final Matrix ma2 = new Matrix(3, 3);
25271         calibrator.getInitialMa(ma2);
25272         assertEquals(ma1, ma2);
25273         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25274         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25275         final NEDPosition nedPosition1 = new NEDPosition();
25276         assertTrue(calibrator.getNedPosition(nedPosition1));
25277         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25278         assertSame(calibrator.getMeasurements(), measurements);
25279         assertFalse(calibrator.isCommonAxisUsed());
25280         assertNull(calibrator.getListener());
25281         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25282         assertFalse(calibrator.isReady());
25283         assertFalse(calibrator.isRunning());
25284         assertNull(calibrator.getEstimatedBiases());
25285         assertFalse(calibrator.getEstimatedBiases(null));
25286         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25287         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25288         assertNull(calibrator.getEstimatedBiasFx());
25289         assertNull(calibrator.getEstimatedBiasFy());
25290         assertNull(calibrator.getEstimatedBiasFz());
25291         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25292         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25293         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25294         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25295         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25296         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25297         assertNull(calibrator.getEstimatedMa());
25298         assertNull(calibrator.getEstimatedSx());
25299         assertNull(calibrator.getEstimatedSy());
25300         assertNull(calibrator.getEstimatedSz());
25301         assertNull(calibrator.getEstimatedMxy());
25302         assertNull(calibrator.getEstimatedMxz());
25303         assertNull(calibrator.getEstimatedMyx());
25304         assertNull(calibrator.getEstimatedMyz());
25305         assertNull(calibrator.getEstimatedMzx());
25306         assertNull(calibrator.getEstimatedMzy());
25307         assertNull(calibrator.getEstimatedCovariance());
25308         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25309         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25310                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25311         assertNotNull(calibrator.getGroundTruthGravityNorm());
25312         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25313         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25314         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25315                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25316         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25317         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25318         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25319     }
25320 
25321     @Test
25322     public void testConstructor198() throws WrongSizeException {
25323         final Collection<StandardDeviationBodyKinematics> measurements =
25324                 Collections.emptyList();
25325 
25326         final Matrix ba = generateBa();
25327         final double biasX = ba.getElementAtIndex(0);
25328         final double biasY = ba.getElementAtIndex(1);
25329         final double biasZ = ba.getElementAtIndex(2);
25330 
25331         final Matrix ma = generateMaCommonAxis();
25332         final double sx = ma.getElementAt(0, 0);
25333         final double sy = ma.getElementAt(1, 1);
25334         final double sz = ma.getElementAt(2, 2);
25335         final double mxy = ma.getElementAt(0, 1);
25336         final double mxz = ma.getElementAt(0, 2);
25337         final double myx = ma.getElementAt(1, 0);
25338         final double myz = ma.getElementAt(1, 2);
25339         final double mzx = ma.getElementAt(2, 0);
25340         final double mzy = ma.getElementAt(2, 1);
25341 
25342         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25343         final double latitude = Math.toRadians(
25344                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25345         final double longitude = Math.toRadians(
25346                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25347         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25348         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25349         final NEDVelocity nedVelocity = new NEDVelocity();
25350         final ECEFPosition ecefPosition = new ECEFPosition();
25351         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25352         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25353                 ecefPosition, ecefVelocity);
25354 
25355         final KnownPositionAccelerometerCalibrator calibrator =
25356                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25357                         biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25358                         myx, myz, mzx, mzy, this);
25359 
25360         // check default values
25361         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25362         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25363         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25364         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25365         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25366         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25367         final Acceleration bx2 = new Acceleration(0.0,
25368                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25369         calibrator.getInitialBiasXAsAcceleration(bx2);
25370         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25371         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25372         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25373         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25374         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25375         final Acceleration by2 = new Acceleration(0.0,
25376                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25377         calibrator.getInitialBiasYAsAcceleration(by2);
25378         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25379         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25380         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25381         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25382         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25383         final Acceleration bz2 = new Acceleration(0.0,
25384                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25385         calibrator.getInitialBiasZAsAcceleration(bz2);
25386         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25387         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25388         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25389         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25390         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25391         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25392         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25393         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25394         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25395         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25396         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25397         final double[] bias1 = calibrator.getInitialBias();
25398         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25399         final double[] bias2 = new double[3];
25400         calibrator.getInitialBias(bias2);
25401         assertArrayEquals(bias1, bias2, 0.0);
25402         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25403         assertEquals(b1, ba);
25404         final Matrix b2 = new Matrix(3, 1);
25405         calibrator.getInitialBiasAsMatrix(b2);
25406         assertEquals(b1, b2);
25407         final Matrix ma1 = new Matrix(3, 3);
25408         ma1.setSubmatrix(0, 0,
25409                 2, 2,
25410                 new double[]{sx, myx, mzx,
25411                         mxy, sy, mzy,
25412                         mxz, myz, sz});
25413         assertEquals(calibrator.getInitialMa(), ma1);
25414         final Matrix ma2 = new Matrix(3, 3);
25415         calibrator.getInitialMa(ma2);
25416         assertEquals(ma1, ma2);
25417         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25418         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25419         final NEDPosition nedPosition1 = new NEDPosition();
25420         assertTrue(calibrator.getNedPosition(nedPosition1));
25421         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25422         assertSame(calibrator.getMeasurements(), measurements);
25423         assertFalse(calibrator.isCommonAxisUsed());
25424         assertSame(calibrator.getListener(), this);
25425         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25426         assertFalse(calibrator.isReady());
25427         assertFalse(calibrator.isRunning());
25428         assertNull(calibrator.getEstimatedBiases());
25429         assertFalse(calibrator.getEstimatedBiases(null));
25430         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25431         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25432         assertNull(calibrator.getEstimatedBiasFx());
25433         assertNull(calibrator.getEstimatedBiasFy());
25434         assertNull(calibrator.getEstimatedBiasFz());
25435         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25436         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25437         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25438         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25439         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25440         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25441         assertNull(calibrator.getEstimatedMa());
25442         assertNull(calibrator.getEstimatedSx());
25443         assertNull(calibrator.getEstimatedSy());
25444         assertNull(calibrator.getEstimatedSz());
25445         assertNull(calibrator.getEstimatedMxy());
25446         assertNull(calibrator.getEstimatedMxz());
25447         assertNull(calibrator.getEstimatedMyx());
25448         assertNull(calibrator.getEstimatedMyz());
25449         assertNull(calibrator.getEstimatedMzx());
25450         assertNull(calibrator.getEstimatedMzy());
25451         assertNull(calibrator.getEstimatedCovariance());
25452         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25453         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25454                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25455         assertNotNull(calibrator.getGroundTruthGravityNorm());
25456         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25457         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25458         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25459                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25460         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25461         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25462         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25463     }
25464 
25465     @Test
25466     public void testConstructor199() throws WrongSizeException {
25467         final Matrix ba = generateBa();
25468         final double biasX = ba.getElementAtIndex(0);
25469         final double biasY = ba.getElementAtIndex(1);
25470         final double biasZ = ba.getElementAtIndex(2);
25471 
25472         final Matrix ma = generateMaCommonAxis();
25473         final double sx = ma.getElementAt(0, 0);
25474         final double sy = ma.getElementAt(1, 1);
25475         final double sz = ma.getElementAt(2, 2);
25476         final double mxy = ma.getElementAt(0, 1);
25477         final double mxz = ma.getElementAt(0, 2);
25478         final double myx = ma.getElementAt(1, 0);
25479         final double myz = ma.getElementAt(1, 2);
25480         final double mzx = ma.getElementAt(2, 0);
25481         final double mzy = ma.getElementAt(2, 1);
25482 
25483         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25484         final double latitude = Math.toRadians(
25485                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25486         final double longitude = Math.toRadians(
25487                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25488         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25489         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25490         final NEDVelocity nedVelocity = new NEDVelocity();
25491         final ECEFPosition ecefPosition = new ECEFPosition();
25492         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25493         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25494                 ecefPosition, ecefVelocity);
25495 
25496         final KnownPositionAccelerometerCalibrator calibrator =
25497                 new KnownPositionAccelerometerCalibrator(nedPosition,
25498                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25499                         myx, myz, mzx, mzy);
25500 
25501         // check default values
25502         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25503         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25504         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25505         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25506         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25507         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25508         final Acceleration bx2 = new Acceleration(0.0,
25509                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25510         calibrator.getInitialBiasXAsAcceleration(bx2);
25511         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25512         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25513         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25514         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25515         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25516         final Acceleration by2 = new Acceleration(0.0,
25517                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25518         calibrator.getInitialBiasYAsAcceleration(by2);
25519         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25520         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25521         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25522         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25523         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25524         final Acceleration bz2 = new Acceleration(0.0,
25525                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25526         calibrator.getInitialBiasZAsAcceleration(bz2);
25527         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25528         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25529         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25530         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25531         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25532         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25533         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25534         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25535         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25536         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25537         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25538         final double[] bias1 = calibrator.getInitialBias();
25539         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25540         final double[] bias2 = new double[3];
25541         calibrator.getInitialBias(bias2);
25542         assertArrayEquals(bias1, bias2, 0.0);
25543         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25544         assertEquals(b1, ba);
25545         final Matrix b2 = new Matrix(3, 1);
25546         calibrator.getInitialBiasAsMatrix(b2);
25547         assertEquals(b1, b2);
25548         final Matrix ma1 = new Matrix(3, 3);
25549         ma1.setSubmatrix(0, 0,
25550                 2, 2,
25551                 new double[]{sx, myx, mzx,
25552                         mxy, sy, mzy,
25553                         mxz, myz, sz});
25554         assertEquals(calibrator.getInitialMa(), ma1);
25555         final Matrix ma2 = new Matrix(3, 3);
25556         calibrator.getInitialMa(ma2);
25557         assertEquals(ma1, ma2);
25558         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25559         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25560         final NEDPosition nedPosition1 = new NEDPosition();
25561         assertTrue(calibrator.getNedPosition(nedPosition1));
25562         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25563         assertNull(calibrator.getMeasurements());
25564         assertTrue(calibrator.isCommonAxisUsed());
25565         assertNull(calibrator.getListener());
25566         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25567         assertFalse(calibrator.isReady());
25568         assertFalse(calibrator.isRunning());
25569         assertNull(calibrator.getEstimatedBiases());
25570         assertFalse(calibrator.getEstimatedBiases(null));
25571         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25572         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25573         assertNull(calibrator.getEstimatedBiasFx());
25574         assertNull(calibrator.getEstimatedBiasFy());
25575         assertNull(calibrator.getEstimatedBiasFz());
25576         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25577         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25578         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25579         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25580         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25581         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25582         assertNull(calibrator.getEstimatedMa());
25583         assertNull(calibrator.getEstimatedSx());
25584         assertNull(calibrator.getEstimatedSy());
25585         assertNull(calibrator.getEstimatedSz());
25586         assertNull(calibrator.getEstimatedMxy());
25587         assertNull(calibrator.getEstimatedMxz());
25588         assertNull(calibrator.getEstimatedMyx());
25589         assertNull(calibrator.getEstimatedMyz());
25590         assertNull(calibrator.getEstimatedMzx());
25591         assertNull(calibrator.getEstimatedMzy());
25592         assertNull(calibrator.getEstimatedCovariance());
25593         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25594         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25595                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25596         assertNotNull(calibrator.getGroundTruthGravityNorm());
25597         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25598         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25599         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25600                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25601         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25602         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25603         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25604     }
25605 
25606     @Test
25607     public void testConstructor200() throws WrongSizeException {
25608         final Matrix ba = generateBa();
25609         final double biasX = ba.getElementAtIndex(0);
25610         final double biasY = ba.getElementAtIndex(1);
25611         final double biasZ = ba.getElementAtIndex(2);
25612 
25613         final Matrix ma = generateMaCommonAxis();
25614         final double sx = ma.getElementAt(0, 0);
25615         final double sy = ma.getElementAt(1, 1);
25616         final double sz = ma.getElementAt(2, 2);
25617         final double mxy = ma.getElementAt(0, 1);
25618         final double mxz = ma.getElementAt(0, 2);
25619         final double myx = ma.getElementAt(1, 0);
25620         final double myz = ma.getElementAt(1, 2);
25621         final double mzx = ma.getElementAt(2, 0);
25622         final double mzy = ma.getElementAt(2, 1);
25623 
25624         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25625         final double latitude = Math.toRadians(
25626                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25627         final double longitude = Math.toRadians(
25628                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25629         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25630         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25631         final NEDVelocity nedVelocity = new NEDVelocity();
25632         final ECEFPosition ecefPosition = new ECEFPosition();
25633         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25634         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25635                 ecefPosition, ecefVelocity);
25636 
25637         final KnownPositionAccelerometerCalibrator calibrator =
25638                 new KnownPositionAccelerometerCalibrator(nedPosition,
25639                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25640                         myx, myz, mzx, mzy, this);
25641 
25642         // check default values
25643         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25644         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25645         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25646         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25647         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25648         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25649         final Acceleration bx2 = new Acceleration(0.0,
25650                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25651         calibrator.getInitialBiasXAsAcceleration(bx2);
25652         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25653         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25654         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25655         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25656         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25657         final Acceleration by2 = new Acceleration(0.0,
25658                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25659         calibrator.getInitialBiasYAsAcceleration(by2);
25660         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25661         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25662         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25663         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25664         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25665         final Acceleration bz2 = new Acceleration(0.0,
25666                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25667         calibrator.getInitialBiasZAsAcceleration(bz2);
25668         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25669         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25670         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25671         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25672         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25673         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25674         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25675         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25676         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25677         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25678         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25679         final double[] bias1 = calibrator.getInitialBias();
25680         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25681         final double[] bias2 = new double[3];
25682         calibrator.getInitialBias(bias2);
25683         assertArrayEquals(bias1, bias2, 0.0);
25684         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25685         assertEquals(b1, ba);
25686         final Matrix b2 = new Matrix(3, 1);
25687         calibrator.getInitialBiasAsMatrix(b2);
25688         assertEquals(b1, b2);
25689         final Matrix ma1 = new Matrix(3, 3);
25690         ma1.setSubmatrix(0, 0,
25691                 2, 2,
25692                 new double[]{sx, myx, mzx,
25693                         mxy, sy, mzy,
25694                         mxz, myz, sz});
25695         assertEquals(calibrator.getInitialMa(), ma1);
25696         final Matrix ma2 = new Matrix(3, 3);
25697         calibrator.getInitialMa(ma2);
25698         assertEquals(ma1, ma2);
25699         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25700         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25701         final NEDPosition nedPosition1 = new NEDPosition();
25702         assertTrue(calibrator.getNedPosition(nedPosition1));
25703         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25704         assertNull(calibrator.getMeasurements());
25705         assertTrue(calibrator.isCommonAxisUsed());
25706         assertSame(calibrator.getListener(), this);
25707         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25708         assertFalse(calibrator.isReady());
25709         assertFalse(calibrator.isRunning());
25710         assertNull(calibrator.getEstimatedBiases());
25711         assertFalse(calibrator.getEstimatedBiases(null));
25712         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25713         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25714         assertNull(calibrator.getEstimatedBiasFx());
25715         assertNull(calibrator.getEstimatedBiasFy());
25716         assertNull(calibrator.getEstimatedBiasFz());
25717         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25718         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25719         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25720         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25721         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25722         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25723         assertNull(calibrator.getEstimatedMa());
25724         assertNull(calibrator.getEstimatedSx());
25725         assertNull(calibrator.getEstimatedSy());
25726         assertNull(calibrator.getEstimatedSz());
25727         assertNull(calibrator.getEstimatedMxy());
25728         assertNull(calibrator.getEstimatedMxz());
25729         assertNull(calibrator.getEstimatedMyx());
25730         assertNull(calibrator.getEstimatedMyz());
25731         assertNull(calibrator.getEstimatedMzx());
25732         assertNull(calibrator.getEstimatedMzy());
25733         assertNull(calibrator.getEstimatedCovariance());
25734         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25735         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25736                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25737         assertNotNull(calibrator.getGroundTruthGravityNorm());
25738         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25739         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25740         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25741                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25742         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25743         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25744         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25745     }
25746 
25747     @Test
25748     public void testConstructor201() throws WrongSizeException {
25749         final Collection<StandardDeviationBodyKinematics> measurements =
25750                 Collections.emptyList();
25751 
25752         final Matrix ba = generateBa();
25753         final double biasX = ba.getElementAtIndex(0);
25754         final double biasY = ba.getElementAtIndex(1);
25755         final double biasZ = ba.getElementAtIndex(2);
25756 
25757         final Matrix ma = generateMaCommonAxis();
25758         final double sx = ma.getElementAt(0, 0);
25759         final double sy = ma.getElementAt(1, 1);
25760         final double sz = ma.getElementAt(2, 2);
25761         final double mxy = ma.getElementAt(0, 1);
25762         final double mxz = ma.getElementAt(0, 2);
25763         final double myx = ma.getElementAt(1, 0);
25764         final double myz = ma.getElementAt(1, 2);
25765         final double mzx = ma.getElementAt(2, 0);
25766         final double mzy = ma.getElementAt(2, 1);
25767 
25768         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25769         final double latitude = Math.toRadians(
25770                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25771         final double longitude = Math.toRadians(
25772                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25773         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25774         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25775         final NEDVelocity nedVelocity = new NEDVelocity();
25776         final ECEFPosition ecefPosition = new ECEFPosition();
25777         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25778         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25779                 ecefPosition, ecefVelocity);
25780 
25781         final KnownPositionAccelerometerCalibrator calibrator =
25782                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25783                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25784                         myx, myz, mzx, mzy);
25785 
25786         // check default values
25787         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25788         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25789         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25790         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25791         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25792         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25793         final Acceleration bx2 = new Acceleration(0.0,
25794                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25795         calibrator.getInitialBiasXAsAcceleration(bx2);
25796         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25797         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25798         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25799         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25800         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25801         final Acceleration by2 = new Acceleration(0.0,
25802                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25803         calibrator.getInitialBiasYAsAcceleration(by2);
25804         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25805         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25806         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25807         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25808         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25809         final Acceleration bz2 = new Acceleration(0.0,
25810                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25811         calibrator.getInitialBiasZAsAcceleration(bz2);
25812         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25813         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25814         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25815         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25816         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25817         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25818         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25819         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25820         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25821         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25822         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25823         final double[] bias1 = calibrator.getInitialBias();
25824         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25825         final double[] bias2 = new double[3];
25826         calibrator.getInitialBias(bias2);
25827         assertArrayEquals(bias1, bias2, 0.0);
25828         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25829         assertEquals(b1, ba);
25830         final Matrix b2 = new Matrix(3, 1);
25831         calibrator.getInitialBiasAsMatrix(b2);
25832         assertEquals(b1, b2);
25833         final Matrix ma1 = new Matrix(3, 3);
25834         ma1.setSubmatrix(0, 0,
25835                 2, 2,
25836                 new double[]{sx, myx, mzx,
25837                         mxy, sy, mzy,
25838                         mxz, myz, sz});
25839         assertEquals(calibrator.getInitialMa(), ma1);
25840         final Matrix ma2 = new Matrix(3, 3);
25841         calibrator.getInitialMa(ma2);
25842         assertEquals(ma1, ma2);
25843         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25844         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25845         final NEDPosition nedPosition1 = new NEDPosition();
25846         assertTrue(calibrator.getNedPosition(nedPosition1));
25847         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25848         assertSame(calibrator.getMeasurements(), measurements);
25849         assertTrue(calibrator.isCommonAxisUsed());
25850         assertNull(calibrator.getListener());
25851         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25852         assertFalse(calibrator.isReady());
25853         assertFalse(calibrator.isRunning());
25854         assertNull(calibrator.getEstimatedBiases());
25855         assertFalse(calibrator.getEstimatedBiases(null));
25856         assertNull(calibrator.getEstimatedBiasesAsMatrix());
25857         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25858         assertNull(calibrator.getEstimatedBiasFx());
25859         assertNull(calibrator.getEstimatedBiasFy());
25860         assertNull(calibrator.getEstimatedBiasFz());
25861         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25862         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25863         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25864         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25865         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25866         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25867         assertNull(calibrator.getEstimatedMa());
25868         assertNull(calibrator.getEstimatedSx());
25869         assertNull(calibrator.getEstimatedSy());
25870         assertNull(calibrator.getEstimatedSz());
25871         assertNull(calibrator.getEstimatedMxy());
25872         assertNull(calibrator.getEstimatedMxz());
25873         assertNull(calibrator.getEstimatedMyx());
25874         assertNull(calibrator.getEstimatedMyz());
25875         assertNull(calibrator.getEstimatedMzx());
25876         assertNull(calibrator.getEstimatedMzy());
25877         assertNull(calibrator.getEstimatedCovariance());
25878         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25879         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25880                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25881         assertNotNull(calibrator.getGroundTruthGravityNorm());
25882         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25883         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25884         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25885                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25886         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25887         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25888         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25889     }
25890 
25891     @Test
25892     public void testConstructor202() throws WrongSizeException {
25893         final Collection<StandardDeviationBodyKinematics> measurements =
25894                 Collections.emptyList();
25895 
25896         final Matrix ba = generateBa();
25897         final double biasX = ba.getElementAtIndex(0);
25898         final double biasY = ba.getElementAtIndex(1);
25899         final double biasZ = ba.getElementAtIndex(2);
25900 
25901         final Matrix ma = generateMaCommonAxis();
25902         final double sx = ma.getElementAt(0, 0);
25903         final double sy = ma.getElementAt(1, 1);
25904         final double sz = ma.getElementAt(2, 2);
25905         final double mxy = ma.getElementAt(0, 1);
25906         final double mxz = ma.getElementAt(0, 2);
25907         final double myx = ma.getElementAt(1, 0);
25908         final double myz = ma.getElementAt(1, 2);
25909         final double mzx = ma.getElementAt(2, 0);
25910         final double mzy = ma.getElementAt(2, 1);
25911 
25912         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25913         final double latitude = Math.toRadians(
25914                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25915         final double longitude = Math.toRadians(
25916                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25917         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25918         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25919         final NEDVelocity nedVelocity = new NEDVelocity();
25920         final ECEFPosition ecefPosition = new ECEFPosition();
25921         final ECEFVelocity ecefVelocity = new ECEFVelocity();
25922         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25923                 ecefPosition, ecefVelocity);
25924 
25925         final KnownPositionAccelerometerCalibrator calibrator =
25926                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25927                         true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25928                         myx, myz, mzx, mzy, this);
25929 
25930         // check default values
25931         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25932         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25933         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25934         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25935         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25936         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25937         final Acceleration bx2 = new Acceleration(0.0,
25938                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25939         calibrator.getInitialBiasXAsAcceleration(bx2);
25940         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25941         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25942         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25943         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25944         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25945         final Acceleration by2 = new Acceleration(0.0,
25946                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25947         calibrator.getInitialBiasYAsAcceleration(by2);
25948         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25949         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25950         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25951         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25952         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25953         final Acceleration bz2 = new Acceleration(0.0,
25954                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25955         calibrator.getInitialBiasZAsAcceleration(bz2);
25956         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25957         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25958         assertEquals(calibrator.getInitialSx(), sx, 0.0);
25959         assertEquals(calibrator.getInitialSy(), sy, 0.0);
25960         assertEquals(calibrator.getInitialSz(), sz, 0.0);
25961         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25962         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25963         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25964         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25965         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25966         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25967         final double[] bias1 = calibrator.getInitialBias();
25968         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25969         final double[] bias2 = new double[3];
25970         calibrator.getInitialBias(bias2);
25971         assertArrayEquals(bias1, bias2, 0.0);
25972         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25973         assertEquals(b1, ba);
25974         final Matrix b2 = new Matrix(3, 1);
25975         calibrator.getInitialBiasAsMatrix(b2);
25976         assertEquals(b1, b2);
25977         final Matrix ma1 = new Matrix(3, 3);
25978         ma1.setSubmatrix(0, 0,
25979                 2, 2,
25980                 new double[]{sx, myx, mzx,
25981                         mxy, sy, mzy,
25982                         mxz, myz, sz});
25983         assertEquals(calibrator.getInitialMa(), ma1);
25984         final Matrix ma2 = new Matrix(3, 3);
25985         calibrator.getInitialMa(ma2);
25986         assertEquals(ma1, ma2);
25987         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25988         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25989         final NEDPosition nedPosition1 = new NEDPosition();
25990         assertTrue(calibrator.getNedPosition(nedPosition1));
25991         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25992         assertSame(calibrator.getMeasurements(), measurements);
25993         assertTrue(calibrator.isCommonAxisUsed());
25994         assertSame(calibrator.getListener(), this);
25995         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25996         assertFalse(calibrator.isReady());
25997         assertFalse(calibrator.isRunning());
25998         assertNull(calibrator.getEstimatedBiases());
25999         assertFalse(calibrator.getEstimatedBiases(null));
26000         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26001         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26002         assertNull(calibrator.getEstimatedBiasFx());
26003         assertNull(calibrator.getEstimatedBiasFy());
26004         assertNull(calibrator.getEstimatedBiasFz());
26005         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26006         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26007         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26008         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26009         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26010         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26011         assertNull(calibrator.getEstimatedMa());
26012         assertNull(calibrator.getEstimatedSx());
26013         assertNull(calibrator.getEstimatedSy());
26014         assertNull(calibrator.getEstimatedSz());
26015         assertNull(calibrator.getEstimatedMxy());
26016         assertNull(calibrator.getEstimatedMxz());
26017         assertNull(calibrator.getEstimatedMyx());
26018         assertNull(calibrator.getEstimatedMyz());
26019         assertNull(calibrator.getEstimatedMzx());
26020         assertNull(calibrator.getEstimatedMzy());
26021         assertNull(calibrator.getEstimatedCovariance());
26022         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26023         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26024                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26025         assertNotNull(calibrator.getGroundTruthGravityNorm());
26026         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26027         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26028         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26029                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26030         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26031         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26032         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26033     }
26034 
26035     @Test
26036     public void testConstructor203() throws WrongSizeException {
26037         final Matrix ba = generateBa();
26038         final double biasX = ba.getElementAtIndex(0);
26039         final double biasY = ba.getElementAtIndex(1);
26040         final double biasZ = ba.getElementAtIndex(2);
26041 
26042         final Acceleration bx = new Acceleration(biasX,
26043                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26044         final Acceleration by = new Acceleration(biasY,
26045                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26046         final Acceleration bz = new Acceleration(biasZ,
26047                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26048 
26049         final Matrix ma = generateMaCommonAxis();
26050         final double sx = ma.getElementAt(0, 0);
26051         final double sy = ma.getElementAt(1, 1);
26052         final double sz = ma.getElementAt(2, 2);
26053         final double mxy = ma.getElementAt(0, 1);
26054         final double mxz = ma.getElementAt(0, 2);
26055         final double myx = ma.getElementAt(1, 0);
26056         final double myz = ma.getElementAt(1, 2);
26057         final double mzx = ma.getElementAt(2, 0);
26058         final double mzy = ma.getElementAt(2, 1);
26059 
26060         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26061         final double latitude = Math.toRadians(
26062                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26063         final double longitude = Math.toRadians(
26064                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26065         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26066         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26067         final NEDVelocity nedVelocity = new NEDVelocity();
26068         final ECEFPosition ecefPosition = new ECEFPosition();
26069         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26070         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26071                 ecefPosition, ecefVelocity);
26072 
26073         final KnownPositionAccelerometerCalibrator calibrator =
26074                 new KnownPositionAccelerometerCalibrator(nedPosition,
26075                         bx, by, bz, sx, sy, sz, mxy, mxz,
26076                         myx, myz, mzx, mzy);
26077 
26078         // check default values
26079         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26080         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26081         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26082         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26083         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26084         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26085         final Acceleration bx2 = new Acceleration(0.0,
26086                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26087         calibrator.getInitialBiasXAsAcceleration(bx2);
26088         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26089         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26090         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26091         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26092         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26093         final Acceleration by2 = new Acceleration(0.0,
26094                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26095         calibrator.getInitialBiasYAsAcceleration(by2);
26096         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26097         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26098         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26099         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26100         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26101         final Acceleration bz2 = new Acceleration(0.0,
26102                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26103         calibrator.getInitialBiasZAsAcceleration(bz2);
26104         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26105         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26106         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26107         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26108         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26109         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26110         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26111         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26112         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26113         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26114         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26115         final double[] bias1 = calibrator.getInitialBias();
26116         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26117         final double[] bias2 = new double[3];
26118         calibrator.getInitialBias(bias2);
26119         assertArrayEquals(bias1, bias2, 0.0);
26120         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26121         assertEquals(b1, ba);
26122         final Matrix b2 = new Matrix(3, 1);
26123         calibrator.getInitialBiasAsMatrix(b2);
26124         assertEquals(b1, b2);
26125         final Matrix ma1 = new Matrix(3, 3);
26126         ma1.setSubmatrix(0, 0,
26127                 2, 2,
26128                 new double[]{sx, myx, mzx,
26129                         mxy, sy, mzy,
26130                         mxz, myz, sz});
26131         assertEquals(calibrator.getInitialMa(), ma1);
26132         final Matrix ma2 = new Matrix(3, 3);
26133         calibrator.getInitialMa(ma2);
26134         assertEquals(ma1, ma2);
26135         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26136         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26137         final NEDPosition nedPosition1 = new NEDPosition();
26138         assertTrue(calibrator.getNedPosition(nedPosition1));
26139         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26140         assertNull(calibrator.getMeasurements());
26141         assertFalse(calibrator.isCommonAxisUsed());
26142         assertNull(calibrator.getListener());
26143         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26144         assertFalse(calibrator.isReady());
26145         assertFalse(calibrator.isRunning());
26146         assertNull(calibrator.getEstimatedBiases());
26147         assertFalse(calibrator.getEstimatedBiases(null));
26148         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26149         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26150         assertNull(calibrator.getEstimatedBiasFx());
26151         assertNull(calibrator.getEstimatedBiasFy());
26152         assertNull(calibrator.getEstimatedBiasFz());
26153         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26154         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26155         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26156         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26157         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26158         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26159         assertNull(calibrator.getEstimatedMa());
26160         assertNull(calibrator.getEstimatedSx());
26161         assertNull(calibrator.getEstimatedSy());
26162         assertNull(calibrator.getEstimatedSz());
26163         assertNull(calibrator.getEstimatedMxy());
26164         assertNull(calibrator.getEstimatedMxz());
26165         assertNull(calibrator.getEstimatedMyx());
26166         assertNull(calibrator.getEstimatedMyz());
26167         assertNull(calibrator.getEstimatedMzx());
26168         assertNull(calibrator.getEstimatedMzy());
26169         assertNull(calibrator.getEstimatedCovariance());
26170         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26171         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26172                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26173         assertNotNull(calibrator.getGroundTruthGravityNorm());
26174         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26175         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26176         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26177                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26178         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26179         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26180         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26181     }
26182 
26183     @Test
26184     public void testConstructor204() throws WrongSizeException {
26185         final Matrix ba = generateBa();
26186         final double biasX = ba.getElementAtIndex(0);
26187         final double biasY = ba.getElementAtIndex(1);
26188         final double biasZ = ba.getElementAtIndex(2);
26189 
26190         final Acceleration bx = new Acceleration(biasX,
26191                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26192         final Acceleration by = new Acceleration(biasY,
26193                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26194         final Acceleration bz = new Acceleration(biasZ,
26195                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26196 
26197         final Matrix ma = generateMaCommonAxis();
26198         final double sx = ma.getElementAt(0, 0);
26199         final double sy = ma.getElementAt(1, 1);
26200         final double sz = ma.getElementAt(2, 2);
26201         final double mxy = ma.getElementAt(0, 1);
26202         final double mxz = ma.getElementAt(0, 2);
26203         final double myx = ma.getElementAt(1, 0);
26204         final double myz = ma.getElementAt(1, 2);
26205         final double mzx = ma.getElementAt(2, 0);
26206         final double mzy = ma.getElementAt(2, 1);
26207 
26208         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26209         final double latitude = Math.toRadians(
26210                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26211         final double longitude = Math.toRadians(
26212                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26213         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26214         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26215         final NEDVelocity nedVelocity = new NEDVelocity();
26216         final ECEFPosition ecefPosition = new ECEFPosition();
26217         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26218         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26219                 ecefPosition, ecefVelocity);
26220 
26221         final KnownPositionAccelerometerCalibrator calibrator =
26222                 new KnownPositionAccelerometerCalibrator(nedPosition,
26223                         bx, by, bz, sx, sy, sz, mxy, mxz,
26224                         myx, myz, mzx, mzy, this);
26225 
26226         // check default values
26227         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26228         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26229         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26230         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26231         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26232         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26233         final Acceleration bx2 = new Acceleration(0.0,
26234                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26235         calibrator.getInitialBiasXAsAcceleration(bx2);
26236         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26237         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26238         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26239         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26240         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26241         final Acceleration by2 = new Acceleration(0.0,
26242                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26243         calibrator.getInitialBiasYAsAcceleration(by2);
26244         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26245         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26246         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26247         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26248         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26249         final Acceleration bz2 = new Acceleration(0.0,
26250                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26251         calibrator.getInitialBiasZAsAcceleration(bz2);
26252         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26253         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26254         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26255         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26256         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26257         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26258         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26259         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26260         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26261         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26262         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26263         final double[] bias1 = calibrator.getInitialBias();
26264         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26265         final double[] bias2 = new double[3];
26266         calibrator.getInitialBias(bias2);
26267         assertArrayEquals(bias1, bias2, 0.0);
26268         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26269         assertEquals(b1, ba);
26270         final Matrix b2 = new Matrix(3, 1);
26271         calibrator.getInitialBiasAsMatrix(b2);
26272         assertEquals(b1, b2);
26273         final Matrix ma1 = new Matrix(3, 3);
26274         ma1.setSubmatrix(0, 0,
26275                 2, 2,
26276                 new double[]{sx, myx, mzx,
26277                         mxy, sy, mzy,
26278                         mxz, myz, sz});
26279         assertEquals(calibrator.getInitialMa(), ma1);
26280         final Matrix ma2 = new Matrix(3, 3);
26281         calibrator.getInitialMa(ma2);
26282         assertEquals(ma1, ma2);
26283         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26284         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26285         final NEDPosition nedPosition1 = new NEDPosition();
26286         assertTrue(calibrator.getNedPosition(nedPosition1));
26287         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26288         assertNull(calibrator.getMeasurements());
26289         assertFalse(calibrator.isCommonAxisUsed());
26290         assertSame(calibrator.getListener(), this);
26291         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26292         assertFalse(calibrator.isReady());
26293         assertFalse(calibrator.isRunning());
26294         assertNull(calibrator.getEstimatedBiases());
26295         assertFalse(calibrator.getEstimatedBiases(null));
26296         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26297         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26298         assertNull(calibrator.getEstimatedBiasFx());
26299         assertNull(calibrator.getEstimatedBiasFy());
26300         assertNull(calibrator.getEstimatedBiasFz());
26301         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26302         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26303         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26304         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26305         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26306         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26307         assertNull(calibrator.getEstimatedMa());
26308         assertNull(calibrator.getEstimatedSx());
26309         assertNull(calibrator.getEstimatedSy());
26310         assertNull(calibrator.getEstimatedSz());
26311         assertNull(calibrator.getEstimatedMxy());
26312         assertNull(calibrator.getEstimatedMxz());
26313         assertNull(calibrator.getEstimatedMyx());
26314         assertNull(calibrator.getEstimatedMyz());
26315         assertNull(calibrator.getEstimatedMzx());
26316         assertNull(calibrator.getEstimatedMzy());
26317         assertNull(calibrator.getEstimatedCovariance());
26318         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26319         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26320                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26321         assertNotNull(calibrator.getGroundTruthGravityNorm());
26322         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26323         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26324         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26325                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26326         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26327         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26328         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26329     }
26330 
26331     @Test
26332     public void testConstructor205() throws WrongSizeException {
26333         final Collection<StandardDeviationBodyKinematics> measurements =
26334                 Collections.emptyList();
26335 
26336         final Matrix ba = generateBa();
26337         final double biasX = ba.getElementAtIndex(0);
26338         final double biasY = ba.getElementAtIndex(1);
26339         final double biasZ = ba.getElementAtIndex(2);
26340 
26341         final Acceleration bx = new Acceleration(biasX,
26342                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26343         final Acceleration by = new Acceleration(biasY,
26344                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26345         final Acceleration bz = new Acceleration(biasZ,
26346                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26347 
26348         final Matrix ma = generateMaCommonAxis();
26349         final double sx = ma.getElementAt(0, 0);
26350         final double sy = ma.getElementAt(1, 1);
26351         final double sz = ma.getElementAt(2, 2);
26352         final double mxy = ma.getElementAt(0, 1);
26353         final double mxz = ma.getElementAt(0, 2);
26354         final double myx = ma.getElementAt(1, 0);
26355         final double myz = ma.getElementAt(1, 2);
26356         final double mzx = ma.getElementAt(2, 0);
26357         final double mzy = ma.getElementAt(2, 1);
26358 
26359         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26360         final double latitude = Math.toRadians(
26361                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26362         final double longitude = Math.toRadians(
26363                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26364         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26365         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26366         final NEDVelocity nedVelocity = new NEDVelocity();
26367         final ECEFPosition ecefPosition = new ECEFPosition();
26368         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26369         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26370                 ecefPosition, ecefVelocity);
26371 
26372         final KnownPositionAccelerometerCalibrator calibrator =
26373                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26374                         bx, by, bz, sx, sy, sz, mxy, mxz,
26375                         myx, myz, mzx, mzy);
26376 
26377         // check default values
26378         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26379         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26380         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26381         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26382         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26383         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26384         final Acceleration bx2 = new Acceleration(0.0,
26385                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26386         calibrator.getInitialBiasXAsAcceleration(bx2);
26387         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26388         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26389         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26390         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26391         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26392         final Acceleration by2 = new Acceleration(0.0,
26393                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26394         calibrator.getInitialBiasYAsAcceleration(by2);
26395         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26396         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26397         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26398         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26399         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26400         final Acceleration bz2 = new Acceleration(0.0,
26401                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26402         calibrator.getInitialBiasZAsAcceleration(bz2);
26403         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26404         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26405         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26406         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26407         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26408         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26409         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26410         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26411         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26412         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26413         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26414         final double[] bias1 = calibrator.getInitialBias();
26415         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26416         final double[] bias2 = new double[3];
26417         calibrator.getInitialBias(bias2);
26418         assertArrayEquals(bias1, bias2, 0.0);
26419         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26420         assertEquals(b1, ba);
26421         final Matrix b2 = new Matrix(3, 1);
26422         calibrator.getInitialBiasAsMatrix(b2);
26423         assertEquals(b1, b2);
26424         final Matrix ma1 = new Matrix(3, 3);
26425         ma1.setSubmatrix(0, 0,
26426                 2, 2,
26427                 new double[]{sx, myx, mzx,
26428                         mxy, sy, mzy,
26429                         mxz, myz, sz});
26430         assertEquals(calibrator.getInitialMa(), ma1);
26431         final Matrix ma2 = new Matrix(3, 3);
26432         calibrator.getInitialMa(ma2);
26433         assertEquals(ma1, ma2);
26434         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26435         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26436         final NEDPosition nedPosition1 = new NEDPosition();
26437         assertTrue(calibrator.getNedPosition(nedPosition1));
26438         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26439         assertSame(calibrator.getMeasurements(), measurements);
26440         assertFalse(calibrator.isCommonAxisUsed());
26441         assertNull(calibrator.getListener());
26442         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26443         assertFalse(calibrator.isReady());
26444         assertFalse(calibrator.isRunning());
26445         assertNull(calibrator.getEstimatedBiases());
26446         assertFalse(calibrator.getEstimatedBiases(null));
26447         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26448         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26449         assertNull(calibrator.getEstimatedBiasFx());
26450         assertNull(calibrator.getEstimatedBiasFy());
26451         assertNull(calibrator.getEstimatedBiasFz());
26452         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26453         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26454         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26455         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26456         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26457         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26458         assertNull(calibrator.getEstimatedMa());
26459         assertNull(calibrator.getEstimatedSx());
26460         assertNull(calibrator.getEstimatedSy());
26461         assertNull(calibrator.getEstimatedSz());
26462         assertNull(calibrator.getEstimatedMxy());
26463         assertNull(calibrator.getEstimatedMxz());
26464         assertNull(calibrator.getEstimatedMyx());
26465         assertNull(calibrator.getEstimatedMyz());
26466         assertNull(calibrator.getEstimatedMzx());
26467         assertNull(calibrator.getEstimatedMzy());
26468         assertNull(calibrator.getEstimatedCovariance());
26469         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26470         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26471                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26472         assertNotNull(calibrator.getGroundTruthGravityNorm());
26473         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26474         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26475         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26476                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26477         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26478         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26479         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26480     }
26481 
26482     @Test
26483     public void testConstructor206() throws WrongSizeException {
26484         final Collection<StandardDeviationBodyKinematics> measurements =
26485                 Collections.emptyList();
26486 
26487         final Matrix ba = generateBa();
26488         final double biasX = ba.getElementAtIndex(0);
26489         final double biasY = ba.getElementAtIndex(1);
26490         final double biasZ = ba.getElementAtIndex(2);
26491 
26492         final Acceleration bx = new Acceleration(biasX,
26493                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26494         final Acceleration by = new Acceleration(biasY,
26495                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26496         final Acceleration bz = new Acceleration(biasZ,
26497                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26498 
26499         final Matrix ma = generateMaCommonAxis();
26500         final double sx = ma.getElementAt(0, 0);
26501         final double sy = ma.getElementAt(1, 1);
26502         final double sz = ma.getElementAt(2, 2);
26503         final double mxy = ma.getElementAt(0, 1);
26504         final double mxz = ma.getElementAt(0, 2);
26505         final double myx = ma.getElementAt(1, 0);
26506         final double myz = ma.getElementAt(1, 2);
26507         final double mzx = ma.getElementAt(2, 0);
26508         final double mzy = ma.getElementAt(2, 1);
26509 
26510         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26511         final double latitude = Math.toRadians(
26512                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26513         final double longitude = Math.toRadians(
26514                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26515         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26516         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26517         final NEDVelocity nedVelocity = new NEDVelocity();
26518         final ECEFPosition ecefPosition = new ECEFPosition();
26519         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26520         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26521                 ecefPosition, ecefVelocity);
26522 
26523         final KnownPositionAccelerometerCalibrator calibrator =
26524                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26525                         bx, by, bz, sx, sy, sz, mxy, mxz,
26526                         myx, myz, mzx, mzy, this);
26527 
26528         // check default values
26529         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26530         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26531         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26532         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26533         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26534         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26535         final Acceleration bx2 = new Acceleration(0.0,
26536                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26537         calibrator.getInitialBiasXAsAcceleration(bx2);
26538         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26539         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26540         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26541         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26542         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26543         final Acceleration by2 = new Acceleration(0.0,
26544                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26545         calibrator.getInitialBiasYAsAcceleration(by2);
26546         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26547         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26548         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26549         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26550         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26551         final Acceleration bz2 = new Acceleration(0.0,
26552                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26553         calibrator.getInitialBiasZAsAcceleration(bz2);
26554         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26555         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26556         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26557         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26558         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26559         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26560         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26561         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26562         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26563         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26564         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26565         final double[] bias1 = calibrator.getInitialBias();
26566         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26567         final double[] bias2 = new double[3];
26568         calibrator.getInitialBias(bias2);
26569         assertArrayEquals(bias1, bias2, 0.0);
26570         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26571         assertEquals(b1, ba);
26572         final Matrix b2 = new Matrix(3, 1);
26573         calibrator.getInitialBiasAsMatrix(b2);
26574         assertEquals(b1, b2);
26575         final Matrix ma1 = new Matrix(3, 3);
26576         ma1.setSubmatrix(0, 0,
26577                 2, 2,
26578                 new double[]{sx, myx, mzx,
26579                         mxy, sy, mzy,
26580                         mxz, myz, sz});
26581         assertEquals(calibrator.getInitialMa(), ma1);
26582         final Matrix ma2 = new Matrix(3, 3);
26583         calibrator.getInitialMa(ma2);
26584         assertEquals(ma1, ma2);
26585         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26586         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26587         final NEDPosition nedPosition1 = new NEDPosition();
26588         assertTrue(calibrator.getNedPosition(nedPosition1));
26589         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26590         assertSame(calibrator.getMeasurements(), measurements);
26591         assertFalse(calibrator.isCommonAxisUsed());
26592         assertSame(calibrator.getListener(), this);
26593         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26594         assertFalse(calibrator.isReady());
26595         assertFalse(calibrator.isRunning());
26596         assertNull(calibrator.getEstimatedBiases());
26597         assertFalse(calibrator.getEstimatedBiases(null));
26598         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26599         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26600         assertNull(calibrator.getEstimatedBiasFx());
26601         assertNull(calibrator.getEstimatedBiasFy());
26602         assertNull(calibrator.getEstimatedBiasFz());
26603         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26604         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26605         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26606         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26607         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26608         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26609         assertNull(calibrator.getEstimatedMa());
26610         assertNull(calibrator.getEstimatedSx());
26611         assertNull(calibrator.getEstimatedSy());
26612         assertNull(calibrator.getEstimatedSz());
26613         assertNull(calibrator.getEstimatedMxy());
26614         assertNull(calibrator.getEstimatedMxz());
26615         assertNull(calibrator.getEstimatedMyx());
26616         assertNull(calibrator.getEstimatedMyz());
26617         assertNull(calibrator.getEstimatedMzx());
26618         assertNull(calibrator.getEstimatedMzy());
26619         assertNull(calibrator.getEstimatedCovariance());
26620         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26621         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26622                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26623         assertNotNull(calibrator.getGroundTruthGravityNorm());
26624         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26625         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26626         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26627                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26628         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26629         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26630         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26631     }
26632 
26633     @Test
26634     public void testConstructor207() throws WrongSizeException {
26635         final Matrix ba = generateBa();
26636         final double biasX = ba.getElementAtIndex(0);
26637         final double biasY = ba.getElementAtIndex(1);
26638         final double biasZ = ba.getElementAtIndex(2);
26639 
26640         final Acceleration bx = new Acceleration(biasX,
26641                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26642         final Acceleration by = new Acceleration(biasY,
26643                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26644         final Acceleration bz = new Acceleration(biasZ,
26645                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26646 
26647         final Matrix ma = generateMaCommonAxis();
26648         final double sx = ma.getElementAt(0, 0);
26649         final double sy = ma.getElementAt(1, 1);
26650         final double sz = ma.getElementAt(2, 2);
26651         final double mxy = ma.getElementAt(0, 1);
26652         final double mxz = ma.getElementAt(0, 2);
26653         final double myx = ma.getElementAt(1, 0);
26654         final double myz = ma.getElementAt(1, 2);
26655         final double mzx = ma.getElementAt(2, 0);
26656         final double mzy = ma.getElementAt(2, 1);
26657 
26658         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26659         final double latitude = Math.toRadians(
26660                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26661         final double longitude = Math.toRadians(
26662                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26663         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26664         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26665         final NEDVelocity nedVelocity = new NEDVelocity();
26666         final ECEFPosition ecefPosition = new ECEFPosition();
26667         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26668         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26669                 ecefPosition, ecefVelocity);
26670 
26671         final KnownPositionAccelerometerCalibrator calibrator =
26672                 new KnownPositionAccelerometerCalibrator(nedPosition,
26673                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
26674                         myx, myz, mzx, mzy);
26675 
26676         // check default values
26677         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26678         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26679         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26680         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26681         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26682         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26683         final Acceleration bx2 = new Acceleration(0.0,
26684                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26685         calibrator.getInitialBiasXAsAcceleration(bx2);
26686         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26687         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26688         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26689         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26690         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26691         final Acceleration by2 = new Acceleration(0.0,
26692                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26693         calibrator.getInitialBiasYAsAcceleration(by2);
26694         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26695         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26696         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26697         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26698         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26699         final Acceleration bz2 = new Acceleration(0.0,
26700                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26701         calibrator.getInitialBiasZAsAcceleration(bz2);
26702         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26703         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26704         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26705         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26706         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26707         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26708         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26709         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26710         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26711         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26712         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26713         final double[] bias1 = calibrator.getInitialBias();
26714         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26715         final double[] bias2 = new double[3];
26716         calibrator.getInitialBias(bias2);
26717         assertArrayEquals(bias1, bias2, 0.0);
26718         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26719         assertEquals(b1, ba);
26720         final Matrix b2 = new Matrix(3, 1);
26721         calibrator.getInitialBiasAsMatrix(b2);
26722         assertEquals(b1, b2);
26723         final Matrix ma1 = new Matrix(3, 3);
26724         ma1.setSubmatrix(0, 0,
26725                 2, 2,
26726                 new double[]{sx, myx, mzx,
26727                         mxy, sy, mzy,
26728                         mxz, myz, sz});
26729         assertEquals(calibrator.getInitialMa(), ma1);
26730         final Matrix ma2 = new Matrix(3, 3);
26731         calibrator.getInitialMa(ma2);
26732         assertEquals(ma1, ma2);
26733         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26734         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26735         final NEDPosition nedPosition1 = new NEDPosition();
26736         assertTrue(calibrator.getNedPosition(nedPosition1));
26737         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26738         assertNull(calibrator.getMeasurements());
26739         assertTrue(calibrator.isCommonAxisUsed());
26740         assertNull(calibrator.getListener());
26741         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26742         assertFalse(calibrator.isReady());
26743         assertFalse(calibrator.isRunning());
26744         assertNull(calibrator.getEstimatedBiases());
26745         assertFalse(calibrator.getEstimatedBiases(null));
26746         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26747         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26748         assertNull(calibrator.getEstimatedBiasFx());
26749         assertNull(calibrator.getEstimatedBiasFy());
26750         assertNull(calibrator.getEstimatedBiasFz());
26751         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26752         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26753         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26754         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26755         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26756         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26757         assertNull(calibrator.getEstimatedMa());
26758         assertNull(calibrator.getEstimatedSx());
26759         assertNull(calibrator.getEstimatedSy());
26760         assertNull(calibrator.getEstimatedSz());
26761         assertNull(calibrator.getEstimatedMxy());
26762         assertNull(calibrator.getEstimatedMxz());
26763         assertNull(calibrator.getEstimatedMyx());
26764         assertNull(calibrator.getEstimatedMyz());
26765         assertNull(calibrator.getEstimatedMzx());
26766         assertNull(calibrator.getEstimatedMzy());
26767         assertNull(calibrator.getEstimatedCovariance());
26768         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26769         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26770                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26771         assertNotNull(calibrator.getGroundTruthGravityNorm());
26772         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26773         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26774         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26775                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26776         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26777         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26778         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26779     }
26780 
26781     @Test
26782     public void testConstructor208() throws WrongSizeException {
26783         final Matrix ba = generateBa();
26784         final double biasX = ba.getElementAtIndex(0);
26785         final double biasY = ba.getElementAtIndex(1);
26786         final double biasZ = ba.getElementAtIndex(2);
26787 
26788         final Acceleration bx = new Acceleration(biasX,
26789                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26790         final Acceleration by = new Acceleration(biasY,
26791                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26792         final Acceleration bz = new Acceleration(biasZ,
26793                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26794 
26795         final Matrix ma = generateMaCommonAxis();
26796         final double sx = ma.getElementAt(0, 0);
26797         final double sy = ma.getElementAt(1, 1);
26798         final double sz = ma.getElementAt(2, 2);
26799         final double mxy = ma.getElementAt(0, 1);
26800         final double mxz = ma.getElementAt(0, 2);
26801         final double myx = ma.getElementAt(1, 0);
26802         final double myz = ma.getElementAt(1, 2);
26803         final double mzx = ma.getElementAt(2, 0);
26804         final double mzy = ma.getElementAt(2, 1);
26805 
26806         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26807         final double latitude = Math.toRadians(
26808                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26809         final double longitude = Math.toRadians(
26810                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26811         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26812         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26813         final NEDVelocity nedVelocity = new NEDVelocity();
26814         final ECEFPosition ecefPosition = new ECEFPosition();
26815         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26816         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26817                 ecefPosition, ecefVelocity);
26818 
26819         final KnownPositionAccelerometerCalibrator calibrator =
26820                 new KnownPositionAccelerometerCalibrator(nedPosition,
26821                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
26822                         myx, myz, mzx, mzy, this);
26823 
26824         // check default values
26825         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26826         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26827         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26828         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26829         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26830         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26831         final Acceleration bx2 = new Acceleration(0.0,
26832                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26833         calibrator.getInitialBiasXAsAcceleration(bx2);
26834         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26835         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26836         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26837         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26838         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26839         final Acceleration by2 = new Acceleration(0.0,
26840                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26841         calibrator.getInitialBiasYAsAcceleration(by2);
26842         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26843         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26844         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26845         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26846         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26847         final Acceleration bz2 = new Acceleration(0.0,
26848                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26849         calibrator.getInitialBiasZAsAcceleration(bz2);
26850         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26851         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26852         assertEquals(calibrator.getInitialSx(), sx, 0.0);
26853         assertEquals(calibrator.getInitialSy(), sy, 0.0);
26854         assertEquals(calibrator.getInitialSz(), sz, 0.0);
26855         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26856         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26857         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26858         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26859         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26860         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26861         final double[] bias1 = calibrator.getInitialBias();
26862         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26863         final double[] bias2 = new double[3];
26864         calibrator.getInitialBias(bias2);
26865         assertArrayEquals(bias1, bias2, 0.0);
26866         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26867         assertEquals(b1, ba);
26868         final Matrix b2 = new Matrix(3, 1);
26869         calibrator.getInitialBiasAsMatrix(b2);
26870         assertEquals(b1, b2);
26871         final Matrix ma1 = new Matrix(3, 3);
26872         ma1.setSubmatrix(0, 0,
26873                 2, 2,
26874                 new double[]{sx, myx, mzx,
26875                         mxy, sy, mzy,
26876                         mxz, myz, sz});
26877         assertEquals(calibrator.getInitialMa(), ma1);
26878         final Matrix ma2 = new Matrix(3, 3);
26879         calibrator.getInitialMa(ma2);
26880         assertEquals(ma1, ma2);
26881         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26882         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26883         final NEDPosition nedPosition1 = new NEDPosition();
26884         assertTrue(calibrator.getNedPosition(nedPosition1));
26885         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26886         assertNull(calibrator.getMeasurements());
26887         assertTrue(calibrator.isCommonAxisUsed());
26888         assertSame(calibrator.getListener(), this);
26889         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26890         assertFalse(calibrator.isReady());
26891         assertFalse(calibrator.isRunning());
26892         assertNull(calibrator.getEstimatedBiases());
26893         assertFalse(calibrator.getEstimatedBiases(null));
26894         assertNull(calibrator.getEstimatedBiasesAsMatrix());
26895         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26896         assertNull(calibrator.getEstimatedBiasFx());
26897         assertNull(calibrator.getEstimatedBiasFy());
26898         assertNull(calibrator.getEstimatedBiasFz());
26899         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26900         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26901         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26902         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26903         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26904         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26905         assertNull(calibrator.getEstimatedMa());
26906         assertNull(calibrator.getEstimatedSx());
26907         assertNull(calibrator.getEstimatedSy());
26908         assertNull(calibrator.getEstimatedSz());
26909         assertNull(calibrator.getEstimatedMxy());
26910         assertNull(calibrator.getEstimatedMxz());
26911         assertNull(calibrator.getEstimatedMyx());
26912         assertNull(calibrator.getEstimatedMyz());
26913         assertNull(calibrator.getEstimatedMzx());
26914         assertNull(calibrator.getEstimatedMzy());
26915         assertNull(calibrator.getEstimatedCovariance());
26916         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26917         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26918                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26919         assertNotNull(calibrator.getGroundTruthGravityNorm());
26920         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26921         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26922         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26923                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26924         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26925         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26926         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26927     }
26928 
26929     @Test
26930     public void testConstructor209() throws WrongSizeException {
26931         final Collection<StandardDeviationBodyKinematics> measurements =
26932                 Collections.emptyList();
26933 
26934         final Matrix ba = generateBa();
26935         final double biasX = ba.getElementAtIndex(0);
26936         final double biasY = ba.getElementAtIndex(1);
26937         final double biasZ = ba.getElementAtIndex(2);
26938 
26939         final Acceleration bx = new Acceleration(biasX,
26940                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26941         final Acceleration by = new Acceleration(biasY,
26942                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26943         final Acceleration bz = new Acceleration(biasZ,
26944                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26945 
26946         final Matrix ma = generateMaCommonAxis();
26947         final double sx = ma.getElementAt(0, 0);
26948         final double sy = ma.getElementAt(1, 1);
26949         final double sz = ma.getElementAt(2, 2);
26950         final double mxy = ma.getElementAt(0, 1);
26951         final double mxz = ma.getElementAt(0, 2);
26952         final double myx = ma.getElementAt(1, 0);
26953         final double myz = ma.getElementAt(1, 2);
26954         final double mzx = ma.getElementAt(2, 0);
26955         final double mzy = ma.getElementAt(2, 1);
26956 
26957         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26958         final double latitude = Math.toRadians(
26959                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26960         final double longitude = Math.toRadians(
26961                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26962         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26963         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26964         final NEDVelocity nedVelocity = new NEDVelocity();
26965         final ECEFPosition ecefPosition = new ECEFPosition();
26966         final ECEFVelocity ecefVelocity = new ECEFVelocity();
26967         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26968                 ecefPosition, ecefVelocity);
26969 
26970         final KnownPositionAccelerometerCalibrator calibrator =
26971                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26972                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
26973                         myx, myz, mzx, mzy);
26974 
26975         // check default values
26976         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26977         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26978         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26979         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26980         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26981         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26982         final Acceleration bx2 = new Acceleration(0.0,
26983                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26984         calibrator.getInitialBiasXAsAcceleration(bx2);
26985         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26986         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26987         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26988         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26989         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26990         final Acceleration by2 = new Acceleration(0.0,
26991                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26992         calibrator.getInitialBiasYAsAcceleration(by2);
26993         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26994         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26995         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26996         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26997         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26998         final Acceleration bz2 = new Acceleration(0.0,
26999                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27000         calibrator.getInitialBiasZAsAcceleration(bz2);
27001         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27002         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27003         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27004         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27005         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27006         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27007         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27008         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27009         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27010         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27011         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27012         final double[] bias1 = calibrator.getInitialBias();
27013         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27014         final double[] bias2 = new double[3];
27015         calibrator.getInitialBias(bias2);
27016         assertArrayEquals(bias1, bias2, 0.0);
27017         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27018         assertEquals(b1, ba);
27019         final Matrix b2 = new Matrix(3, 1);
27020         calibrator.getInitialBiasAsMatrix(b2);
27021         assertEquals(b1, b2);
27022         final Matrix ma1 = new Matrix(3, 3);
27023         ma1.setSubmatrix(0, 0,
27024                 2, 2,
27025                 new double[]{sx, myx, mzx,
27026                         mxy, sy, mzy,
27027                         mxz, myz, sz});
27028         assertEquals(calibrator.getInitialMa(), ma1);
27029         final Matrix ma2 = new Matrix(3, 3);
27030         calibrator.getInitialMa(ma2);
27031         assertEquals(ma1, ma2);
27032         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27033         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27034         final NEDPosition nedPosition1 = new NEDPosition();
27035         assertTrue(calibrator.getNedPosition(nedPosition1));
27036         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27037         assertSame(calibrator.getMeasurements(), measurements);
27038         assertTrue(calibrator.isCommonAxisUsed());
27039         assertNull(calibrator.getListener());
27040         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27041         assertFalse(calibrator.isReady());
27042         assertFalse(calibrator.isRunning());
27043         assertNull(calibrator.getEstimatedBiases());
27044         assertFalse(calibrator.getEstimatedBiases(null));
27045         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27046         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27047         assertNull(calibrator.getEstimatedBiasFx());
27048         assertNull(calibrator.getEstimatedBiasFy());
27049         assertNull(calibrator.getEstimatedBiasFz());
27050         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27051         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27052         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27053         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27054         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27055         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27056         assertNull(calibrator.getEstimatedMa());
27057         assertNull(calibrator.getEstimatedSx());
27058         assertNull(calibrator.getEstimatedSy());
27059         assertNull(calibrator.getEstimatedSz());
27060         assertNull(calibrator.getEstimatedMxy());
27061         assertNull(calibrator.getEstimatedMxz());
27062         assertNull(calibrator.getEstimatedMyx());
27063         assertNull(calibrator.getEstimatedMyz());
27064         assertNull(calibrator.getEstimatedMzx());
27065         assertNull(calibrator.getEstimatedMzy());
27066         assertNull(calibrator.getEstimatedCovariance());
27067         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27068         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27069                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27070         assertNotNull(calibrator.getGroundTruthGravityNorm());
27071         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27072         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27073         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27074                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27075         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27076         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27077         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27078     }
27079 
27080     @Test
27081     public void testConstructor210() throws WrongSizeException {
27082         final Collection<StandardDeviationBodyKinematics> measurements =
27083                 Collections.emptyList();
27084 
27085         final Matrix ba = generateBa();
27086         final double biasX = ba.getElementAtIndex(0);
27087         final double biasY = ba.getElementAtIndex(1);
27088         final double biasZ = ba.getElementAtIndex(2);
27089 
27090         final Acceleration bx = new Acceleration(biasX,
27091                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27092         final Acceleration by = new Acceleration(biasY,
27093                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27094         final Acceleration bz = new Acceleration(biasZ,
27095                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27096 
27097         final Matrix ma = generateMaCommonAxis();
27098         final double sx = ma.getElementAt(0, 0);
27099         final double sy = ma.getElementAt(1, 1);
27100         final double sz = ma.getElementAt(2, 2);
27101         final double mxy = ma.getElementAt(0, 1);
27102         final double mxz = ma.getElementAt(0, 2);
27103         final double myx = ma.getElementAt(1, 0);
27104         final double myz = ma.getElementAt(1, 2);
27105         final double mzx = ma.getElementAt(2, 0);
27106         final double mzy = ma.getElementAt(2, 1);
27107 
27108         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27109         final double latitude = Math.toRadians(
27110                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27111         final double longitude = Math.toRadians(
27112                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27113         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27114         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27115         final NEDVelocity nedVelocity = new NEDVelocity();
27116         final ECEFPosition ecefPosition = new ECEFPosition();
27117         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27118         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27119                 ecefPosition, ecefVelocity);
27120 
27121         final KnownPositionAccelerometerCalibrator calibrator =
27122                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
27123                         true, bx, by, bz, sx, sy, sz, mxy, mxz,
27124                         myx, myz, mzx, mzy, this);
27125 
27126         // check default values
27127         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27128         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27129         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27130         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27131         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27132         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27133         final Acceleration bx2 = new Acceleration(0.0,
27134                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27135         calibrator.getInitialBiasXAsAcceleration(bx2);
27136         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27137         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27138         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27139         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27140         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27141         final Acceleration by2 = new Acceleration(0.0,
27142                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27143         calibrator.getInitialBiasYAsAcceleration(by2);
27144         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27145         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27146         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27147         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27148         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27149         final Acceleration bz2 = new Acceleration(0.0,
27150                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27151         calibrator.getInitialBiasZAsAcceleration(bz2);
27152         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27153         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27154         assertEquals(calibrator.getInitialSx(), sx, 0.0);
27155         assertEquals(calibrator.getInitialSy(), sy, 0.0);
27156         assertEquals(calibrator.getInitialSz(), sz, 0.0);
27157         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27158         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27159         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27160         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27161         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27162         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27163         final double[] bias1 = calibrator.getInitialBias();
27164         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27165         final double[] bias2 = new double[3];
27166         calibrator.getInitialBias(bias2);
27167         assertArrayEquals(bias1, bias2, 0.0);
27168         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27169         assertEquals(b1, ba);
27170         final Matrix b2 = new Matrix(3, 1);
27171         calibrator.getInitialBiasAsMatrix(b2);
27172         assertEquals(b1, b2);
27173         final Matrix ma1 = new Matrix(3, 3);
27174         ma1.setSubmatrix(0, 0,
27175                 2, 2,
27176                 new double[]{sx, myx, mzx,
27177                         mxy, sy, mzy,
27178                         mxz, myz, sz});
27179         assertEquals(calibrator.getInitialMa(), ma1);
27180         final Matrix ma2 = new Matrix(3, 3);
27181         calibrator.getInitialMa(ma2);
27182         assertEquals(ma1, ma2);
27183         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27184         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27185         final NEDPosition nedPosition1 = new NEDPosition();
27186         assertTrue(calibrator.getNedPosition(nedPosition1));
27187         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27188         assertSame(calibrator.getMeasurements(), measurements);
27189         assertTrue(calibrator.isCommonAxisUsed());
27190         assertSame(calibrator.getListener(), this);
27191         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27192         assertFalse(calibrator.isReady());
27193         assertFalse(calibrator.isRunning());
27194         assertNull(calibrator.getEstimatedBiases());
27195         assertFalse(calibrator.getEstimatedBiases(null));
27196         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27197         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27198         assertNull(calibrator.getEstimatedBiasFx());
27199         assertNull(calibrator.getEstimatedBiasFy());
27200         assertNull(calibrator.getEstimatedBiasFz());
27201         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27202         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27203         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27204         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27205         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27206         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27207         assertNull(calibrator.getEstimatedMa());
27208         assertNull(calibrator.getEstimatedSx());
27209         assertNull(calibrator.getEstimatedSy());
27210         assertNull(calibrator.getEstimatedSz());
27211         assertNull(calibrator.getEstimatedMxy());
27212         assertNull(calibrator.getEstimatedMxz());
27213         assertNull(calibrator.getEstimatedMyx());
27214         assertNull(calibrator.getEstimatedMyz());
27215         assertNull(calibrator.getEstimatedMzx());
27216         assertNull(calibrator.getEstimatedMzy());
27217         assertNull(calibrator.getEstimatedCovariance());
27218         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27219         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27220                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27221         assertNotNull(calibrator.getGroundTruthGravityNorm());
27222         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27223         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27224         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27225                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27226         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27227         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27228         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27229     }
27230 
27231     @Test
27232     public void testConstructor211() throws WrongSizeException {
27233         final Matrix ba = generateBa();
27234         final double[] bias = ba.getBuffer();
27235         final double biasX = ba.getElementAtIndex(0);
27236         final double biasY = ba.getElementAtIndex(1);
27237         final double biasZ = ba.getElementAtIndex(2);
27238 
27239         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27240         final double latitude = Math.toRadians(
27241                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27242         final double longitude = Math.toRadians(
27243                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27244         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27245         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27246         final NEDVelocity nedVelocity = new NEDVelocity();
27247         final ECEFPosition ecefPosition = new ECEFPosition();
27248         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27249         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27250                 ecefPosition, ecefVelocity);
27251 
27252         KnownPositionAccelerometerCalibrator calibrator =
27253                 new KnownPositionAccelerometerCalibrator(nedPosition, bias);
27254 
27255         // check default values
27256         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27257         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27258         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27259         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27260         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27261         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27262         final Acceleration bx2 = new Acceleration(0.0,
27263                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27264         calibrator.getInitialBiasXAsAcceleration(bx2);
27265         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27266         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27267         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27268         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27269         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27270         final Acceleration by2 = new Acceleration(0.0,
27271                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27272         calibrator.getInitialBiasYAsAcceleration(by2);
27273         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27274         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27275         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27276         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27277         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27278         final Acceleration bz2 = new Acceleration(0.0,
27279                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27280         calibrator.getInitialBiasZAsAcceleration(bz2);
27281         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27282         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27283         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27284         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27285         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27286         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27287         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27288         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27289         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27290         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27291         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27292         final double[] bias1 = calibrator.getInitialBias();
27293         assertArrayEquals(bias1, bias, 0.0);
27294         final double[] bias2 = new double[3];
27295         calibrator.getInitialBias(bias2);
27296         assertArrayEquals(bias1, bias2, 0.0);
27297         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27298         assertEquals(b1, ba);
27299         final Matrix b2 = new Matrix(3, 1);
27300         calibrator.getInitialBiasAsMatrix(b2);
27301         assertEquals(b1, b2);
27302         final Matrix ma1 = calibrator.getInitialMa();
27303         assertEquals(ma1, new Matrix(3, 3));
27304         final Matrix ma2 = new Matrix(3, 3);
27305         calibrator.getInitialMa(ma2);
27306         assertEquals(ma1, ma2);
27307         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27308         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27309         final NEDPosition nedPosition1 = new NEDPosition();
27310         assertTrue(calibrator.getNedPosition(nedPosition1));
27311         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27312         assertNull(calibrator.getMeasurements());
27313         assertFalse(calibrator.isCommonAxisUsed());
27314         assertNull(calibrator.getListener());
27315         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27316         assertFalse(calibrator.isReady());
27317         assertFalse(calibrator.isRunning());
27318         assertNull(calibrator.getEstimatedBiases());
27319         assertFalse(calibrator.getEstimatedBiases(null));
27320         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27321         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27322         assertNull(calibrator.getEstimatedBiasFx());
27323         assertNull(calibrator.getEstimatedBiasFy());
27324         assertNull(calibrator.getEstimatedBiasFz());
27325         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27326         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27327         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27328         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27329         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27330         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27331         assertNull(calibrator.getEstimatedMa());
27332         assertNull(calibrator.getEstimatedSx());
27333         assertNull(calibrator.getEstimatedSy());
27334         assertNull(calibrator.getEstimatedSz());
27335         assertNull(calibrator.getEstimatedMxy());
27336         assertNull(calibrator.getEstimatedMxz());
27337         assertNull(calibrator.getEstimatedMyx());
27338         assertNull(calibrator.getEstimatedMyz());
27339         assertNull(calibrator.getEstimatedMzx());
27340         assertNull(calibrator.getEstimatedMzy());
27341         assertNull(calibrator.getEstimatedCovariance());
27342         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27343         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27344                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27345         assertNotNull(calibrator.getGroundTruthGravityNorm());
27346         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27347         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27348         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27349                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27350         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27351         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27352         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27353 
27354         // Force IllegalArgumentException
27355         calibrator = null;
27356         try {
27357             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27358                     new double[1]);
27359             fail("IllegalArgumentException expected but not thrown");
27360         } catch (final IllegalArgumentException ignore) {
27361         }
27362         assertNull(calibrator);
27363     }
27364 
27365     @Test
27366     public void testConstructor212() throws WrongSizeException {
27367         final Matrix ba = generateBa();
27368         final double[] bias = ba.getBuffer();
27369         final double biasX = ba.getElementAtIndex(0);
27370         final double biasY = ba.getElementAtIndex(1);
27371         final double biasZ = ba.getElementAtIndex(2);
27372 
27373         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27374         final double latitude = Math.toRadians(
27375                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27376         final double longitude = Math.toRadians(
27377                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27378         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27379         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27380         final NEDVelocity nedVelocity = new NEDVelocity();
27381         final ECEFPosition ecefPosition = new ECEFPosition();
27382         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27383         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27384                 ecefPosition, ecefVelocity);
27385 
27386         KnownPositionAccelerometerCalibrator calibrator =
27387                 new KnownPositionAccelerometerCalibrator(nedPosition, bias,
27388                         this);
27389 
27390         // check default values
27391         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27392         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27393         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27394         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27395         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27396         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27397         final Acceleration bx2 = new Acceleration(0.0,
27398                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27399         calibrator.getInitialBiasXAsAcceleration(bx2);
27400         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27401         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27402         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27403         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27404         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27405         final Acceleration by2 = new Acceleration(0.0,
27406                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27407         calibrator.getInitialBiasYAsAcceleration(by2);
27408         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27409         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27410         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27411         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27412         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27413         final Acceleration bz2 = new Acceleration(0.0,
27414                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27415         calibrator.getInitialBiasZAsAcceleration(bz2);
27416         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27417         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27418         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27419         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27420         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27421         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27422         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27423         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27424         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27425         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27426         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27427         final double[] bias1 = calibrator.getInitialBias();
27428         assertArrayEquals(bias1, bias, 0.0);
27429         final double[] bias2 = new double[3];
27430         calibrator.getInitialBias(bias2);
27431         assertArrayEquals(bias1, bias2, 0.0);
27432         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27433         assertEquals(b1, ba);
27434         final Matrix b2 = new Matrix(3, 1);
27435         calibrator.getInitialBiasAsMatrix(b2);
27436         assertEquals(b1, b2);
27437         final Matrix ma1 = calibrator.getInitialMa();
27438         assertEquals(ma1, new Matrix(3, 3));
27439         final Matrix ma2 = new Matrix(3, 3);
27440         calibrator.getInitialMa(ma2);
27441         assertEquals(ma1, ma2);
27442         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27443         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27444         final NEDPosition nedPosition1 = new NEDPosition();
27445         assertTrue(calibrator.getNedPosition(nedPosition1));
27446         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27447         assertNull(calibrator.getMeasurements());
27448         assertFalse(calibrator.isCommonAxisUsed());
27449         assertSame(calibrator.getListener(), this);
27450         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27451         assertFalse(calibrator.isReady());
27452         assertFalse(calibrator.isRunning());
27453         assertNull(calibrator.getEstimatedBiases());
27454         assertFalse(calibrator.getEstimatedBiases(null));
27455         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27456         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27457         assertNull(calibrator.getEstimatedBiasFx());
27458         assertNull(calibrator.getEstimatedBiasFy());
27459         assertNull(calibrator.getEstimatedBiasFz());
27460         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27461         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27462         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27463         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27464         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27465         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27466         assertNull(calibrator.getEstimatedMa());
27467         assertNull(calibrator.getEstimatedSx());
27468         assertNull(calibrator.getEstimatedSy());
27469         assertNull(calibrator.getEstimatedSz());
27470         assertNull(calibrator.getEstimatedMxy());
27471         assertNull(calibrator.getEstimatedMxz());
27472         assertNull(calibrator.getEstimatedMyx());
27473         assertNull(calibrator.getEstimatedMyz());
27474         assertNull(calibrator.getEstimatedMzx());
27475         assertNull(calibrator.getEstimatedMzy());
27476         assertNull(calibrator.getEstimatedCovariance());
27477         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27478         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27479                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27480         assertNotNull(calibrator.getGroundTruthGravityNorm());
27481         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27482         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27483         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27484                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27485         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27486         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27487         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27488 
27489         // Force IllegalArgumentException
27490         calibrator = null;
27491         try {
27492             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27493                     new double[1], this);
27494             fail("IllegalArgumentException expected but not thrown");
27495         } catch (final IllegalArgumentException ignore) {
27496         }
27497         assertNull(calibrator);
27498     }
27499 
27500     @Test
27501     public void testConstructor213() throws WrongSizeException {
27502         final Collection<StandardDeviationBodyKinematics> measurements =
27503                 Collections.emptyList();
27504 
27505         final Matrix ba = generateBa();
27506         final double[] bias = ba.getBuffer();
27507         final double biasX = ba.getElementAtIndex(0);
27508         final double biasY = ba.getElementAtIndex(1);
27509         final double biasZ = ba.getElementAtIndex(2);
27510 
27511         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27512         final double latitude = Math.toRadians(
27513                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27514         final double longitude = Math.toRadians(
27515                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27516         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27517         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27518         final NEDVelocity nedVelocity = new NEDVelocity();
27519         final ECEFPosition ecefPosition = new ECEFPosition();
27520         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27521         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27522                 ecefPosition, ecefVelocity);
27523 
27524         KnownPositionAccelerometerCalibrator calibrator =
27525                 new KnownPositionAccelerometerCalibrator(nedPosition,
27526                         measurements, bias);
27527 
27528         // check default values
27529         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27530         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27531         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27532         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27533         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27534         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27535         final Acceleration bx2 = new Acceleration(0.0,
27536                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27537         calibrator.getInitialBiasXAsAcceleration(bx2);
27538         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27539         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27540         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27541         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27542         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27543         final Acceleration by2 = new Acceleration(0.0,
27544                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27545         calibrator.getInitialBiasYAsAcceleration(by2);
27546         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27547         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27548         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27549         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27550         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27551         final Acceleration bz2 = new Acceleration(0.0,
27552                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27553         calibrator.getInitialBiasZAsAcceleration(bz2);
27554         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27555         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27556         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27557         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27558         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27559         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27560         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27561         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27562         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27563         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27564         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27565         final double[] bias1 = calibrator.getInitialBias();
27566         assertArrayEquals(bias1, bias, 0.0);
27567         final double[] bias2 = new double[3];
27568         calibrator.getInitialBias(bias2);
27569         assertArrayEquals(bias1, bias2, 0.0);
27570         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27571         assertEquals(b1, ba);
27572         final Matrix b2 = new Matrix(3, 1);
27573         calibrator.getInitialBiasAsMatrix(b2);
27574         assertEquals(b1, b2);
27575         final Matrix ma1 = calibrator.getInitialMa();
27576         assertEquals(ma1, new Matrix(3, 3));
27577         final Matrix ma2 = new Matrix(3, 3);
27578         calibrator.getInitialMa(ma2);
27579         assertEquals(ma1, ma2);
27580         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27581         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27582         final NEDPosition nedPosition1 = new NEDPosition();
27583         assertTrue(calibrator.getNedPosition(nedPosition1));
27584         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27585         assertSame(calibrator.getMeasurements(), measurements);
27586         assertFalse(calibrator.isCommonAxisUsed());
27587         assertNull(calibrator.getListener());
27588         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27589         assertFalse(calibrator.isReady());
27590         assertFalse(calibrator.isRunning());
27591         assertNull(calibrator.getEstimatedBiases());
27592         assertFalse(calibrator.getEstimatedBiases(null));
27593         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27594         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27595         assertNull(calibrator.getEstimatedBiasFx());
27596         assertNull(calibrator.getEstimatedBiasFy());
27597         assertNull(calibrator.getEstimatedBiasFz());
27598         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27599         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27600         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27601         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27602         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27603         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27604         assertNull(calibrator.getEstimatedMa());
27605         assertNull(calibrator.getEstimatedSx());
27606         assertNull(calibrator.getEstimatedSy());
27607         assertNull(calibrator.getEstimatedSz());
27608         assertNull(calibrator.getEstimatedMxy());
27609         assertNull(calibrator.getEstimatedMxz());
27610         assertNull(calibrator.getEstimatedMyx());
27611         assertNull(calibrator.getEstimatedMyz());
27612         assertNull(calibrator.getEstimatedMzx());
27613         assertNull(calibrator.getEstimatedMzy());
27614         assertNull(calibrator.getEstimatedCovariance());
27615         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27616         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27617                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27618         assertNotNull(calibrator.getGroundTruthGravityNorm());
27619         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27620         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27621         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27622                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27623         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27624         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27625         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27626 
27627         // Force IllegalArgumentException
27628         calibrator = null;
27629         try {
27630             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27631                     measurements, new double[1]);
27632             fail("IllegalArgumentException expected but not thrown");
27633         } catch (final IllegalArgumentException ignore) {
27634         }
27635         assertNull(calibrator);
27636     }
27637 
27638     @Test
27639     public void testConstructor214() throws WrongSizeException {
27640         final Collection<StandardDeviationBodyKinematics> measurements =
27641                 Collections.emptyList();
27642 
27643         final Matrix ba = generateBa();
27644         final double[] bias = ba.getBuffer();
27645         final double biasX = ba.getElementAtIndex(0);
27646         final double biasY = ba.getElementAtIndex(1);
27647         final double biasZ = ba.getElementAtIndex(2);
27648 
27649         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27650         final double latitude = Math.toRadians(
27651                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27652         final double longitude = Math.toRadians(
27653                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27654         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27655         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27656         final NEDVelocity nedVelocity = new NEDVelocity();
27657         final ECEFPosition ecefPosition = new ECEFPosition();
27658         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27659         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27660                 ecefPosition, ecefVelocity);
27661 
27662         KnownPositionAccelerometerCalibrator calibrator =
27663                 new KnownPositionAccelerometerCalibrator(nedPosition,
27664                         measurements, bias, this);
27665 
27666         // check default values
27667         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27668         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27669         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27670         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27671         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27672         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27673         final Acceleration bx2 = new Acceleration(0.0,
27674                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27675         calibrator.getInitialBiasXAsAcceleration(bx2);
27676         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27677         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27678         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27679         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27680         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27681         final Acceleration by2 = new Acceleration(0.0,
27682                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27683         calibrator.getInitialBiasYAsAcceleration(by2);
27684         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27685         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27686         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27687         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27688         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27689         final Acceleration bz2 = new Acceleration(0.0,
27690                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27691         calibrator.getInitialBiasZAsAcceleration(bz2);
27692         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27693         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27694         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27695         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27696         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27697         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27698         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27699         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27700         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27701         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27702         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27703         final double[] bias1 = calibrator.getInitialBias();
27704         assertArrayEquals(bias1, bias, 0.0);
27705         final double[] bias2 = new double[3];
27706         calibrator.getInitialBias(bias2);
27707         assertArrayEquals(bias1, bias2, 0.0);
27708         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27709         assertEquals(b1, ba);
27710         final Matrix b2 = new Matrix(3, 1);
27711         calibrator.getInitialBiasAsMatrix(b2);
27712         assertEquals(b1, b2);
27713         final Matrix ma1 = calibrator.getInitialMa();
27714         assertEquals(ma1, new Matrix(3, 3));
27715         final Matrix ma2 = new Matrix(3, 3);
27716         calibrator.getInitialMa(ma2);
27717         assertEquals(ma1, ma2);
27718         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27719         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27720         final NEDPosition nedPosition1 = new NEDPosition();
27721         assertTrue(calibrator.getNedPosition(nedPosition1));
27722         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27723         assertSame(calibrator.getMeasurements(), measurements);
27724         assertFalse(calibrator.isCommonAxisUsed());
27725         assertSame(calibrator.getListener(), this);
27726         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27727         assertFalse(calibrator.isReady());
27728         assertFalse(calibrator.isRunning());
27729         assertNull(calibrator.getEstimatedBiases());
27730         assertFalse(calibrator.getEstimatedBiases(null));
27731         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27732         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27733         assertNull(calibrator.getEstimatedBiasFx());
27734         assertNull(calibrator.getEstimatedBiasFy());
27735         assertNull(calibrator.getEstimatedBiasFz());
27736         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27737         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27738         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27739         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27740         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27741         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27742         assertNull(calibrator.getEstimatedMa());
27743         assertNull(calibrator.getEstimatedSx());
27744         assertNull(calibrator.getEstimatedSy());
27745         assertNull(calibrator.getEstimatedSz());
27746         assertNull(calibrator.getEstimatedMxy());
27747         assertNull(calibrator.getEstimatedMxz());
27748         assertNull(calibrator.getEstimatedMyx());
27749         assertNull(calibrator.getEstimatedMyz());
27750         assertNull(calibrator.getEstimatedMzx());
27751         assertNull(calibrator.getEstimatedMzy());
27752         assertNull(calibrator.getEstimatedCovariance());
27753         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27754         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27755                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27756         assertNotNull(calibrator.getGroundTruthGravityNorm());
27757         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27758         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27759         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27760                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27761         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27762         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27763         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27764 
27765         // Force IllegalArgumentException
27766         calibrator = null;
27767         try {
27768             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27769                     measurements, new double[1], this);
27770             fail("IllegalArgumentException expected but not thrown");
27771         } catch (final IllegalArgumentException ignore) {
27772         }
27773         assertNull(calibrator);
27774     }
27775 
27776     @Test
27777     public void testConstructor215() throws WrongSizeException {
27778         final Matrix ba = generateBa();
27779         final double[] bias = ba.getBuffer();
27780         final double biasX = ba.getElementAtIndex(0);
27781         final double biasY = ba.getElementAtIndex(1);
27782         final double biasZ = ba.getElementAtIndex(2);
27783 
27784         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27785         final double latitude = Math.toRadians(
27786                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27787         final double longitude = Math.toRadians(
27788                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27789         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27790         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27791         final NEDVelocity nedVelocity = new NEDVelocity();
27792         final ECEFPosition ecefPosition = new ECEFPosition();
27793         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27794         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27795                 ecefPosition, ecefVelocity);
27796 
27797         KnownPositionAccelerometerCalibrator calibrator =
27798                 new KnownPositionAccelerometerCalibrator(nedPosition,
27799                         true, bias);
27800 
27801         // check default values
27802         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27803         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27804         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27805         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27806         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27807         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27808         final Acceleration bx2 = new Acceleration(0.0,
27809                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27810         calibrator.getInitialBiasXAsAcceleration(bx2);
27811         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27812         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27813         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27814         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27815         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27816         final Acceleration by2 = new Acceleration(0.0,
27817                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27818         calibrator.getInitialBiasYAsAcceleration(by2);
27819         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27820         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27821         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27822         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27823         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27824         final Acceleration bz2 = new Acceleration(0.0,
27825                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27826         calibrator.getInitialBiasZAsAcceleration(bz2);
27827         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27828         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27829         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27830         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27831         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27832         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27833         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27834         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27835         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27836         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27837         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27838         final double[] bias1 = calibrator.getInitialBias();
27839         assertArrayEquals(bias1, bias, 0.0);
27840         final double[] bias2 = new double[3];
27841         calibrator.getInitialBias(bias2);
27842         assertArrayEquals(bias1, bias2, 0.0);
27843         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27844         assertEquals(b1, ba);
27845         final Matrix b2 = new Matrix(3, 1);
27846         calibrator.getInitialBiasAsMatrix(b2);
27847         assertEquals(b1, b2);
27848         final Matrix ma1 = calibrator.getInitialMa();
27849         assertEquals(ma1, new Matrix(3, 3));
27850         final Matrix ma2 = new Matrix(3, 3);
27851         calibrator.getInitialMa(ma2);
27852         assertEquals(ma1, ma2);
27853         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27854         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27855         final NEDPosition nedPosition1 = new NEDPosition();
27856         assertTrue(calibrator.getNedPosition(nedPosition1));
27857         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27858         assertNull(calibrator.getMeasurements());
27859         assertTrue(calibrator.isCommonAxisUsed());
27860         assertNull(calibrator.getListener());
27861         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27862         assertFalse(calibrator.isReady());
27863         assertFalse(calibrator.isRunning());
27864         assertNull(calibrator.getEstimatedBiases());
27865         assertFalse(calibrator.getEstimatedBiases(null));
27866         assertNull(calibrator.getEstimatedBiasesAsMatrix());
27867         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27868         assertNull(calibrator.getEstimatedBiasFx());
27869         assertNull(calibrator.getEstimatedBiasFy());
27870         assertNull(calibrator.getEstimatedBiasFz());
27871         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27872         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27873         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27874         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27875         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27876         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27877         assertNull(calibrator.getEstimatedMa());
27878         assertNull(calibrator.getEstimatedSx());
27879         assertNull(calibrator.getEstimatedSy());
27880         assertNull(calibrator.getEstimatedSz());
27881         assertNull(calibrator.getEstimatedMxy());
27882         assertNull(calibrator.getEstimatedMxz());
27883         assertNull(calibrator.getEstimatedMyx());
27884         assertNull(calibrator.getEstimatedMyz());
27885         assertNull(calibrator.getEstimatedMzx());
27886         assertNull(calibrator.getEstimatedMzy());
27887         assertNull(calibrator.getEstimatedCovariance());
27888         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27889         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27890                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27891         assertNotNull(calibrator.getGroundTruthGravityNorm());
27892         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27893         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27894         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27895                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27896         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27897         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27898         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27899 
27900         // Force IllegalArgumentException
27901         calibrator = null;
27902         try {
27903             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27904                     true, new double[1]);
27905             fail("IllegalArgumentException expected but not thrown");
27906         } catch (final IllegalArgumentException ignore) {
27907         }
27908         assertNull(calibrator);
27909     }
27910 
27911     @Test
27912     public void testConstructor216() throws WrongSizeException {
27913         final Matrix ba = generateBa();
27914         final double[] bias = ba.getBuffer();
27915         final double biasX = ba.getElementAtIndex(0);
27916         final double biasY = ba.getElementAtIndex(1);
27917         final double biasZ = ba.getElementAtIndex(2);
27918 
27919         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27920         final double latitude = Math.toRadians(
27921                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27922         final double longitude = Math.toRadians(
27923                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27924         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27925         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27926         final NEDVelocity nedVelocity = new NEDVelocity();
27927         final ECEFPosition ecefPosition = new ECEFPosition();
27928         final ECEFVelocity ecefVelocity = new ECEFVelocity();
27929         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27930                 ecefPosition, ecefVelocity);
27931 
27932         KnownPositionAccelerometerCalibrator calibrator =
27933                 new KnownPositionAccelerometerCalibrator(nedPosition,
27934                         true, bias, this);
27935 
27936         // check default values
27937         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27938         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27939         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27940         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27941         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27942         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27943         final Acceleration bx2 = new Acceleration(0.0,
27944                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27945         calibrator.getInitialBiasXAsAcceleration(bx2);
27946         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27947         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27948         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27949         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27950         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27951         final Acceleration by2 = new Acceleration(0.0,
27952                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27953         calibrator.getInitialBiasYAsAcceleration(by2);
27954         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27955         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27956         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27957         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27958         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27959         final Acceleration bz2 = new Acceleration(0.0,
27960                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27961         calibrator.getInitialBiasZAsAcceleration(bz2);
27962         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27963         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27964         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27965         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27966         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27967         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27968         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27969         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27970         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27971         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27972         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27973         final double[] bias1 = calibrator.getInitialBias();
27974         assertArrayEquals(bias1, bias, 0.0);
27975         final double[] bias2 = new double[3];
27976         calibrator.getInitialBias(bias2);
27977         assertArrayEquals(bias1, bias2, 0.0);
27978         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27979         assertEquals(b1, ba);
27980         final Matrix b2 = new Matrix(3, 1);
27981         calibrator.getInitialBiasAsMatrix(b2);
27982         assertEquals(b1, b2);
27983         final Matrix ma1 = calibrator.getInitialMa();
27984         assertEquals(ma1, new Matrix(3, 3));
27985         final Matrix ma2 = new Matrix(3, 3);
27986         calibrator.getInitialMa(ma2);
27987         assertEquals(ma1, ma2);
27988         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27989         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27990         final NEDPosition nedPosition1 = new NEDPosition();
27991         assertTrue(calibrator.getNedPosition(nedPosition1));
27992         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27993         assertNull(calibrator.getMeasurements());
27994         assertTrue(calibrator.isCommonAxisUsed());
27995         assertSame(calibrator.getListener(), this);
27996         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27997         assertFalse(calibrator.isReady());
27998         assertFalse(calibrator.isRunning());
27999         assertNull(calibrator.getEstimatedBiases());
28000         assertFalse(calibrator.getEstimatedBiases(null));
28001         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28002         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28003         assertNull(calibrator.getEstimatedBiasFx());
28004         assertNull(calibrator.getEstimatedBiasFy());
28005         assertNull(calibrator.getEstimatedBiasFz());
28006         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28007         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28008         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28009         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28010         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28011         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28012         assertNull(calibrator.getEstimatedMa());
28013         assertNull(calibrator.getEstimatedSx());
28014         assertNull(calibrator.getEstimatedSy());
28015         assertNull(calibrator.getEstimatedSz());
28016         assertNull(calibrator.getEstimatedMxy());
28017         assertNull(calibrator.getEstimatedMxz());
28018         assertNull(calibrator.getEstimatedMyx());
28019         assertNull(calibrator.getEstimatedMyz());
28020         assertNull(calibrator.getEstimatedMzx());
28021         assertNull(calibrator.getEstimatedMzy());
28022         assertNull(calibrator.getEstimatedCovariance());
28023         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28024         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28025                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28026         assertNotNull(calibrator.getGroundTruthGravityNorm());
28027         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28028         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28029         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28030                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28031         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28032         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28033         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28034 
28035         // Force IllegalArgumentException
28036         calibrator = null;
28037         try {
28038             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28039                     true, new double[1], this);
28040             fail("IllegalArgumentException expected but not thrown");
28041         } catch (final IllegalArgumentException ignore) {
28042         }
28043         assertNull(calibrator);
28044     }
28045 
28046     @Test
28047     public void testConstructor217() throws WrongSizeException {
28048         final Collection<StandardDeviationBodyKinematics> measurements =
28049                 Collections.emptyList();
28050 
28051         final Matrix ba = generateBa();
28052         final double[] bias = ba.getBuffer();
28053         final double biasX = ba.getElementAtIndex(0);
28054         final double biasY = ba.getElementAtIndex(1);
28055         final double biasZ = ba.getElementAtIndex(2);
28056 
28057         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28058         final double latitude = Math.toRadians(
28059                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28060         final double longitude = Math.toRadians(
28061                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28062         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28063         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28064         final NEDVelocity nedVelocity = new NEDVelocity();
28065         final ECEFPosition ecefPosition = new ECEFPosition();
28066         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28067         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28068                 ecefPosition, ecefVelocity);
28069 
28070         KnownPositionAccelerometerCalibrator calibrator =
28071                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28072                         true, bias);
28073 
28074         // check default values
28075         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28076         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28077         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28078         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28079         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28080         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28081         final Acceleration bx2 = new Acceleration(0.0,
28082                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28083         calibrator.getInitialBiasXAsAcceleration(bx2);
28084         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28085         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28086         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28087         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28088         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28089         final Acceleration by2 = new Acceleration(0.0,
28090                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28091         calibrator.getInitialBiasYAsAcceleration(by2);
28092         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28093         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28094         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28095         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28096         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28097         final Acceleration bz2 = new Acceleration(0.0,
28098                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28099         calibrator.getInitialBiasZAsAcceleration(bz2);
28100         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28101         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28102         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28103         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28104         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28105         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28106         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28107         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28108         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28109         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28110         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28111         final double[] bias1 = calibrator.getInitialBias();
28112         assertArrayEquals(bias1, bias, 0.0);
28113         final double[] bias2 = new double[3];
28114         calibrator.getInitialBias(bias2);
28115         assertArrayEquals(bias1, bias2, 0.0);
28116         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28117         assertEquals(b1, ba);
28118         final Matrix b2 = new Matrix(3, 1);
28119         calibrator.getInitialBiasAsMatrix(b2);
28120         assertEquals(b1, b2);
28121         final Matrix ma1 = calibrator.getInitialMa();
28122         assertEquals(ma1, new Matrix(3, 3));
28123         final Matrix ma2 = new Matrix(3, 3);
28124         calibrator.getInitialMa(ma2);
28125         assertEquals(ma1, ma2);
28126         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28127         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28128         final NEDPosition nedPosition1 = new NEDPosition();
28129         assertTrue(calibrator.getNedPosition(nedPosition1));
28130         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28131         assertSame(calibrator.getMeasurements(), measurements);
28132         assertTrue(calibrator.isCommonAxisUsed());
28133         assertNull(calibrator.getListener());
28134         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28135         assertFalse(calibrator.isReady());
28136         assertFalse(calibrator.isRunning());
28137         assertNull(calibrator.getEstimatedBiases());
28138         assertFalse(calibrator.getEstimatedBiases(null));
28139         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28140         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28141         assertNull(calibrator.getEstimatedBiasFx());
28142         assertNull(calibrator.getEstimatedBiasFy());
28143         assertNull(calibrator.getEstimatedBiasFz());
28144         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28145         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28146         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28147         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28148         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28149         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28150         assertNull(calibrator.getEstimatedMa());
28151         assertNull(calibrator.getEstimatedSx());
28152         assertNull(calibrator.getEstimatedSy());
28153         assertNull(calibrator.getEstimatedSz());
28154         assertNull(calibrator.getEstimatedMxy());
28155         assertNull(calibrator.getEstimatedMxz());
28156         assertNull(calibrator.getEstimatedMyx());
28157         assertNull(calibrator.getEstimatedMyz());
28158         assertNull(calibrator.getEstimatedMzx());
28159         assertNull(calibrator.getEstimatedMzy());
28160         assertNull(calibrator.getEstimatedCovariance());
28161         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28162         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28163                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28164         assertNotNull(calibrator.getGroundTruthGravityNorm());
28165         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28166         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28167         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28168                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28169         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28170         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28171         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28172 
28173         // Force IllegalArgumentException
28174         calibrator = null;
28175         try {
28176             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28177                     measurements, true, new double[1]);
28178             fail("IllegalArgumentException expected but not thrown");
28179         } catch (final IllegalArgumentException ignore) {
28180         }
28181         assertNull(calibrator);
28182     }
28183 
28184     @Test
28185     public void testConstructor218() throws WrongSizeException {
28186         final Collection<StandardDeviationBodyKinematics> measurements =
28187                 Collections.emptyList();
28188 
28189         final Matrix ba = generateBa();
28190         final double[] bias = ba.getBuffer();
28191         final double biasX = ba.getElementAtIndex(0);
28192         final double biasY = ba.getElementAtIndex(1);
28193         final double biasZ = ba.getElementAtIndex(2);
28194 
28195         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28196         final double latitude = Math.toRadians(
28197                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28198         final double longitude = Math.toRadians(
28199                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28200         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28201         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28202         final NEDVelocity nedVelocity = new NEDVelocity();
28203         final ECEFPosition ecefPosition = new ECEFPosition();
28204         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28205         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28206                 ecefPosition, ecefVelocity);
28207 
28208         KnownPositionAccelerometerCalibrator calibrator =
28209                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28210                         true, bias, this);
28211 
28212         // check default values
28213         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28214         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28215         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28216         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28217         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28218         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28219         final Acceleration bx2 = new Acceleration(0.0,
28220                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28221         calibrator.getInitialBiasXAsAcceleration(bx2);
28222         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28223         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28224         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28225         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28226         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28227         final Acceleration by2 = new Acceleration(0.0,
28228                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28229         calibrator.getInitialBiasYAsAcceleration(by2);
28230         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28231         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28232         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28233         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28234         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28235         final Acceleration bz2 = new Acceleration(0.0,
28236                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28237         calibrator.getInitialBiasZAsAcceleration(bz2);
28238         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28239         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28240         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28241         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28242         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28243         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28244         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28245         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28246         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28247         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28248         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28249         final double[] bias1 = calibrator.getInitialBias();
28250         assertArrayEquals(bias1, bias, 0.0);
28251         final double[] bias2 = new double[3];
28252         calibrator.getInitialBias(bias2);
28253         assertArrayEquals(bias1, bias2, 0.0);
28254         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28255         assertEquals(b1, ba);
28256         final Matrix b2 = new Matrix(3, 1);
28257         calibrator.getInitialBiasAsMatrix(b2);
28258         assertEquals(b1, b2);
28259         final Matrix ma1 = calibrator.getInitialMa();
28260         assertEquals(ma1, new Matrix(3, 3));
28261         final Matrix ma2 = new Matrix(3, 3);
28262         calibrator.getInitialMa(ma2);
28263         assertEquals(ma1, ma2);
28264         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28265         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28266         final NEDPosition nedPosition1 = new NEDPosition();
28267         assertTrue(calibrator.getNedPosition(nedPosition1));
28268         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28269         assertSame(calibrator.getMeasurements(), measurements);
28270         assertTrue(calibrator.isCommonAxisUsed());
28271         assertSame(calibrator.getListener(), this);
28272         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28273         assertFalse(calibrator.isReady());
28274         assertFalse(calibrator.isRunning());
28275         assertNull(calibrator.getEstimatedBiases());
28276         assertFalse(calibrator.getEstimatedBiases(null));
28277         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28278         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28279         assertNull(calibrator.getEstimatedBiasFx());
28280         assertNull(calibrator.getEstimatedBiasFy());
28281         assertNull(calibrator.getEstimatedBiasFz());
28282         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28283         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28284         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28285         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28286         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28287         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28288         assertNull(calibrator.getEstimatedMa());
28289         assertNull(calibrator.getEstimatedSx());
28290         assertNull(calibrator.getEstimatedSy());
28291         assertNull(calibrator.getEstimatedSz());
28292         assertNull(calibrator.getEstimatedMxy());
28293         assertNull(calibrator.getEstimatedMxz());
28294         assertNull(calibrator.getEstimatedMyx());
28295         assertNull(calibrator.getEstimatedMyz());
28296         assertNull(calibrator.getEstimatedMzx());
28297         assertNull(calibrator.getEstimatedMzy());
28298         assertNull(calibrator.getEstimatedCovariance());
28299         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28300         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28301                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28302         assertNotNull(calibrator.getGroundTruthGravityNorm());
28303         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28304         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28305         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28306                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28307         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28308         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28309         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28310 
28311         // Force IllegalArgumentException
28312         calibrator = null;
28313         try {
28314             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28315                     measurements, true, new double[1], this);
28316             fail("IllegalArgumentException expected but not thrown");
28317         } catch (final IllegalArgumentException ignore) {
28318         }
28319         assertNull(calibrator);
28320     }
28321 
28322     @Test
28323     public void testConstructor219() throws WrongSizeException {
28324         final Matrix ba = generateBa();
28325         final double[] bias = ba.getBuffer();
28326         final double biasX = ba.getElementAtIndex(0);
28327         final double biasY = ba.getElementAtIndex(1);
28328         final double biasZ = ba.getElementAtIndex(2);
28329 
28330         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28331         final double latitude = Math.toRadians(
28332                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28333         final double longitude = Math.toRadians(
28334                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28335         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28336         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28337         final NEDVelocity nedVelocity = new NEDVelocity();
28338         final ECEFPosition ecefPosition = new ECEFPosition();
28339         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28340         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28341                 ecefPosition, ecefVelocity);
28342 
28343         KnownPositionAccelerometerCalibrator calibrator =
28344                 new KnownPositionAccelerometerCalibrator(nedPosition, ba);
28345 
28346         // check default values
28347         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28348         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28349         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28350         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28351         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28352         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28353         final Acceleration bx2 = new Acceleration(0.0,
28354                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28355         calibrator.getInitialBiasXAsAcceleration(bx2);
28356         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28357         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28358         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28359         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28360         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28361         final Acceleration by2 = new Acceleration(0.0,
28362                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28363         calibrator.getInitialBiasYAsAcceleration(by2);
28364         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28365         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28366         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28367         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28368         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28369         final Acceleration bz2 = new Acceleration(0.0,
28370                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28371         calibrator.getInitialBiasZAsAcceleration(bz2);
28372         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28373         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28374         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28375         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28376         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28377         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28378         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28379         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28380         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28381         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28382         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28383         final double[] bias1 = calibrator.getInitialBias();
28384         assertArrayEquals(bias1, bias, 0.0);
28385         final double[] bias2 = new double[3];
28386         calibrator.getInitialBias(bias2);
28387         assertArrayEquals(bias1, bias2, 0.0);
28388         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28389         assertEquals(b1, ba);
28390         final Matrix b2 = new Matrix(3, 1);
28391         calibrator.getInitialBiasAsMatrix(b2);
28392         assertEquals(b1, b2);
28393         final Matrix ma1 = calibrator.getInitialMa();
28394         assertEquals(ma1, new Matrix(3, 3));
28395         final Matrix ma2 = new Matrix(3, 3);
28396         calibrator.getInitialMa(ma2);
28397         assertEquals(ma1, ma2);
28398         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28399         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28400         final NEDPosition nedPosition1 = new NEDPosition();
28401         assertTrue(calibrator.getNedPosition(nedPosition1));
28402         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28403         assertNull(calibrator.getMeasurements());
28404         assertFalse(calibrator.isCommonAxisUsed());
28405         assertNull(calibrator.getListener());
28406         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28407         assertFalse(calibrator.isReady());
28408         assertFalse(calibrator.isRunning());
28409         assertNull(calibrator.getEstimatedBiases());
28410         assertFalse(calibrator.getEstimatedBiases(null));
28411         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28412         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28413         assertNull(calibrator.getEstimatedBiasFx());
28414         assertNull(calibrator.getEstimatedBiasFy());
28415         assertNull(calibrator.getEstimatedBiasFz());
28416         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28417         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28418         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28419         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28420         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28421         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28422         assertNull(calibrator.getEstimatedMa());
28423         assertNull(calibrator.getEstimatedSx());
28424         assertNull(calibrator.getEstimatedSy());
28425         assertNull(calibrator.getEstimatedSz());
28426         assertNull(calibrator.getEstimatedMxy());
28427         assertNull(calibrator.getEstimatedMxz());
28428         assertNull(calibrator.getEstimatedMyx());
28429         assertNull(calibrator.getEstimatedMyz());
28430         assertNull(calibrator.getEstimatedMzx());
28431         assertNull(calibrator.getEstimatedMzy());
28432         assertNull(calibrator.getEstimatedCovariance());
28433         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28434         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28435                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28436         assertNotNull(calibrator.getGroundTruthGravityNorm());
28437         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28438         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28439         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28440                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28441         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28442         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28443         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28444 
28445         // Force IllegalArgumentException
28446         calibrator = null;
28447         try {
28448             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28449                     new Matrix(1, 1));
28450             fail("IllegalArgumentException expected but not thrown");
28451         } catch (final IllegalArgumentException ignore) {
28452         }
28453         try {
28454             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28455                     new Matrix(1, 3));
28456             fail("IllegalArgumentException expected but not thrown");
28457         } catch (final IllegalArgumentException ignore) {
28458         }
28459         assertNull(calibrator);
28460     }
28461 
28462     @Test
28463     public void testConstructor220() throws WrongSizeException {
28464         final Matrix ba = generateBa();
28465         final double[] bias = ba.getBuffer();
28466         final double biasX = ba.getElementAtIndex(0);
28467         final double biasY = ba.getElementAtIndex(1);
28468         final double biasZ = ba.getElementAtIndex(2);
28469 
28470         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28471         final double latitude = Math.toRadians(
28472                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28473         final double longitude = Math.toRadians(
28474                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28475         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28476         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28477         final NEDVelocity nedVelocity = new NEDVelocity();
28478         final ECEFPosition ecefPosition = new ECEFPosition();
28479         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28480         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28481                 ecefPosition, ecefVelocity);
28482 
28483         KnownPositionAccelerometerCalibrator calibrator =
28484                 new KnownPositionAccelerometerCalibrator(nedPosition, ba,
28485                         this);
28486 
28487         // check default values
28488         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28489         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28490         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28491         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28492         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28493         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28494         final Acceleration bx2 = new Acceleration(0.0,
28495                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28496         calibrator.getInitialBiasXAsAcceleration(bx2);
28497         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28498         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28499         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28500         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28501         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28502         final Acceleration by2 = new Acceleration(0.0,
28503                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28504         calibrator.getInitialBiasYAsAcceleration(by2);
28505         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28506         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28507         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28508         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28509         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28510         final Acceleration bz2 = new Acceleration(0.0,
28511                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28512         calibrator.getInitialBiasZAsAcceleration(bz2);
28513         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28514         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28515         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28516         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28517         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28518         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28519         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28520         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28521         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28522         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28523         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28524         final double[] bias1 = calibrator.getInitialBias();
28525         assertArrayEquals(bias1, bias, 0.0);
28526         final double[] bias2 = new double[3];
28527         calibrator.getInitialBias(bias2);
28528         assertArrayEquals(bias1, bias2, 0.0);
28529         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28530         assertEquals(b1, ba);
28531         final Matrix b2 = new Matrix(3, 1);
28532         calibrator.getInitialBiasAsMatrix(b2);
28533         assertEquals(b1, b2);
28534         final Matrix ma1 = calibrator.getInitialMa();
28535         assertEquals(ma1, new Matrix(3, 3));
28536         final Matrix ma2 = new Matrix(3, 3);
28537         calibrator.getInitialMa(ma2);
28538         assertEquals(ma1, ma2);
28539         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28540         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28541         final NEDPosition nedPosition1 = new NEDPosition();
28542         assertTrue(calibrator.getNedPosition(nedPosition1));
28543         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28544         assertNull(calibrator.getMeasurements());
28545         assertFalse(calibrator.isCommonAxisUsed());
28546         assertSame(calibrator.getListener(), this);
28547         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28548         assertFalse(calibrator.isReady());
28549         assertFalse(calibrator.isRunning());
28550         assertNull(calibrator.getEstimatedBiases());
28551         assertFalse(calibrator.getEstimatedBiases(null));
28552         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28553         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28554         assertNull(calibrator.getEstimatedBiasFx());
28555         assertNull(calibrator.getEstimatedBiasFy());
28556         assertNull(calibrator.getEstimatedBiasFz());
28557         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28558         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28559         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28560         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28561         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28562         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28563         assertNull(calibrator.getEstimatedMa());
28564         assertNull(calibrator.getEstimatedSx());
28565         assertNull(calibrator.getEstimatedSy());
28566         assertNull(calibrator.getEstimatedSz());
28567         assertNull(calibrator.getEstimatedMxy());
28568         assertNull(calibrator.getEstimatedMxz());
28569         assertNull(calibrator.getEstimatedMyx());
28570         assertNull(calibrator.getEstimatedMyz());
28571         assertNull(calibrator.getEstimatedMzx());
28572         assertNull(calibrator.getEstimatedMzy());
28573         assertNull(calibrator.getEstimatedCovariance());
28574         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28575         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28576                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28577         assertNotNull(calibrator.getGroundTruthGravityNorm());
28578         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28579         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28580         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28581                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28582         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28583         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28584         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28585 
28586         // Force IllegalArgumentException
28587         calibrator = null;
28588         try {
28589             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28590                     new Matrix(1, 1), this);
28591             fail("IllegalArgumentException expected but not thrown");
28592         } catch (final IllegalArgumentException ignore) {
28593         }
28594         try {
28595             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28596                     new Matrix(1, 3), this);
28597             fail("IllegalArgumentException expected but not thrown");
28598         } catch (final IllegalArgumentException ignore) {
28599         }
28600         assertNull(calibrator);
28601     }
28602 
28603     @Test
28604     public void testConstructor221() throws WrongSizeException {
28605         final Collection<StandardDeviationBodyKinematics> measurements =
28606                 Collections.emptyList();
28607 
28608         final Matrix ba = generateBa();
28609         final double[] bias = ba.getBuffer();
28610         final double biasX = ba.getElementAtIndex(0);
28611         final double biasY = ba.getElementAtIndex(1);
28612         final double biasZ = ba.getElementAtIndex(2);
28613 
28614         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28615         final double latitude = Math.toRadians(
28616                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28617         final double longitude = Math.toRadians(
28618                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28619         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28620         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28621         final NEDVelocity nedVelocity = new NEDVelocity();
28622         final ECEFPosition ecefPosition = new ECEFPosition();
28623         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28624         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28625                 ecefPosition, ecefVelocity);
28626 
28627         KnownPositionAccelerometerCalibrator calibrator =
28628                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28629                         ba);
28630 
28631         // check default values
28632         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28633         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28634         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28635         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28636         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28637         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28638         final Acceleration bx2 = new Acceleration(0.0,
28639                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28640         calibrator.getInitialBiasXAsAcceleration(bx2);
28641         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28642         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28643         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28644         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28645         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28646         final Acceleration by2 = new Acceleration(0.0,
28647                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28648         calibrator.getInitialBiasYAsAcceleration(by2);
28649         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28650         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28651         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28652         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28653         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28654         final Acceleration bz2 = new Acceleration(0.0,
28655                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28656         calibrator.getInitialBiasZAsAcceleration(bz2);
28657         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28658         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28659         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28660         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28661         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28662         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28663         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28664         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28665         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28666         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28667         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28668         final double[] bias1 = calibrator.getInitialBias();
28669         assertArrayEquals(bias1, bias, 0.0);
28670         final double[] bias2 = new double[3];
28671         calibrator.getInitialBias(bias2);
28672         assertArrayEquals(bias1, bias2, 0.0);
28673         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28674         assertEquals(b1, ba);
28675         final Matrix b2 = new Matrix(3, 1);
28676         calibrator.getInitialBiasAsMatrix(b2);
28677         assertEquals(b1, b2);
28678         final Matrix ma1 = calibrator.getInitialMa();
28679         assertEquals(ma1, new Matrix(3, 3));
28680         final Matrix ma2 = new Matrix(3, 3);
28681         calibrator.getInitialMa(ma2);
28682         assertEquals(ma1, ma2);
28683         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28684         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28685         final NEDPosition nedPosition1 = new NEDPosition();
28686         assertTrue(calibrator.getNedPosition(nedPosition1));
28687         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28688         assertSame(calibrator.getMeasurements(), measurements);
28689         assertFalse(calibrator.isCommonAxisUsed());
28690         assertNull(calibrator.getListener());
28691         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28692         assertFalse(calibrator.isReady());
28693         assertFalse(calibrator.isRunning());
28694         assertNull(calibrator.getEstimatedBiases());
28695         assertFalse(calibrator.getEstimatedBiases(null));
28696         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28697         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28698         assertNull(calibrator.getEstimatedBiasFx());
28699         assertNull(calibrator.getEstimatedBiasFy());
28700         assertNull(calibrator.getEstimatedBiasFz());
28701         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28702         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28703         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28704         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28705         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28706         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28707         assertNull(calibrator.getEstimatedMa());
28708         assertNull(calibrator.getEstimatedSx());
28709         assertNull(calibrator.getEstimatedSy());
28710         assertNull(calibrator.getEstimatedSz());
28711         assertNull(calibrator.getEstimatedMxy());
28712         assertNull(calibrator.getEstimatedMxz());
28713         assertNull(calibrator.getEstimatedMyx());
28714         assertNull(calibrator.getEstimatedMyz());
28715         assertNull(calibrator.getEstimatedMzx());
28716         assertNull(calibrator.getEstimatedMzy());
28717         assertNull(calibrator.getEstimatedCovariance());
28718         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28719         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28720                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28721         assertNotNull(calibrator.getGroundTruthGravityNorm());
28722         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28723         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28724         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28725                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28726         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28727         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28728         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28729 
28730         // Force IllegalArgumentException
28731         calibrator = null;
28732         try {
28733             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28734                     measurements, new Matrix(1, 1));
28735             fail("IllegalArgumentException expected but not thrown");
28736         } catch (final IllegalArgumentException ignore) {
28737         }
28738         try {
28739             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28740                     measurements, new Matrix(1, 3));
28741             fail("IllegalArgumentException expected but not thrown");
28742         } catch (final IllegalArgumentException ignore) {
28743         }
28744         assertNull(calibrator);
28745     }
28746 
28747     @Test
28748     public void testConstructor222() throws WrongSizeException {
28749         final Collection<StandardDeviationBodyKinematics> measurements =
28750                 Collections.emptyList();
28751 
28752         final Matrix ba = generateBa();
28753         final double[] bias = ba.getBuffer();
28754         final double biasX = ba.getElementAtIndex(0);
28755         final double biasY = ba.getElementAtIndex(1);
28756         final double biasZ = ba.getElementAtIndex(2);
28757 
28758         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28759         final double latitude = Math.toRadians(
28760                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28761         final double longitude = Math.toRadians(
28762                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28763         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28764         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28765         final NEDVelocity nedVelocity = new NEDVelocity();
28766         final ECEFPosition ecefPosition = new ECEFPosition();
28767         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28768         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28769                 ecefPosition, ecefVelocity);
28770 
28771         KnownPositionAccelerometerCalibrator calibrator =
28772                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28773                         ba, this);
28774 
28775         // check default values
28776         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28777         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28778         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28779         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28780         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28781         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28782         final Acceleration bx2 = new Acceleration(0.0,
28783                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28784         calibrator.getInitialBiasXAsAcceleration(bx2);
28785         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28786         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28787         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28788         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28789         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28790         final Acceleration by2 = new Acceleration(0.0,
28791                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28792         calibrator.getInitialBiasYAsAcceleration(by2);
28793         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28794         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28795         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28796         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28797         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28798         final Acceleration bz2 = new Acceleration(0.0,
28799                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28800         calibrator.getInitialBiasZAsAcceleration(bz2);
28801         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28802         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28803         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28804         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28805         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28806         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28807         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28808         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28809         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28810         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28811         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28812         final double[] bias1 = calibrator.getInitialBias();
28813         assertArrayEquals(bias1, bias, 0.0);
28814         final double[] bias2 = new double[3];
28815         calibrator.getInitialBias(bias2);
28816         assertArrayEquals(bias1, bias2, 0.0);
28817         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28818         assertEquals(b1, ba);
28819         final Matrix b2 = new Matrix(3, 1);
28820         calibrator.getInitialBiasAsMatrix(b2);
28821         assertEquals(b1, b2);
28822         final Matrix ma1 = calibrator.getInitialMa();
28823         assertEquals(ma1, new Matrix(3, 3));
28824         final Matrix ma2 = new Matrix(3, 3);
28825         calibrator.getInitialMa(ma2);
28826         assertEquals(ma1, ma2);
28827         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28828         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28829         final NEDPosition nedPosition1 = new NEDPosition();
28830         assertTrue(calibrator.getNedPosition(nedPosition1));
28831         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28832         assertSame(calibrator.getMeasurements(), measurements);
28833         assertFalse(calibrator.isCommonAxisUsed());
28834         assertSame(calibrator.getListener(), this);
28835         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28836         assertFalse(calibrator.isReady());
28837         assertFalse(calibrator.isRunning());
28838         assertNull(calibrator.getEstimatedBiases());
28839         assertFalse(calibrator.getEstimatedBiases(null));
28840         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28841         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28842         assertNull(calibrator.getEstimatedBiasFx());
28843         assertNull(calibrator.getEstimatedBiasFy());
28844         assertNull(calibrator.getEstimatedBiasFz());
28845         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28846         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28847         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28848         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28849         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28850         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28851         assertNull(calibrator.getEstimatedMa());
28852         assertNull(calibrator.getEstimatedSx());
28853         assertNull(calibrator.getEstimatedSy());
28854         assertNull(calibrator.getEstimatedSz());
28855         assertNull(calibrator.getEstimatedMxy());
28856         assertNull(calibrator.getEstimatedMxz());
28857         assertNull(calibrator.getEstimatedMyx());
28858         assertNull(calibrator.getEstimatedMyz());
28859         assertNull(calibrator.getEstimatedMzx());
28860         assertNull(calibrator.getEstimatedMzy());
28861         assertNull(calibrator.getEstimatedCovariance());
28862         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28863         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28864                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28865         assertNotNull(calibrator.getGroundTruthGravityNorm());
28866         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28867         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28868         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28869                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28870         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28871         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28872         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28873 
28874         // Force IllegalArgumentException
28875         calibrator = null;
28876         try {
28877             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28878                     measurements, new Matrix(1, 1), this);
28879             fail("IllegalArgumentException expected but not thrown");
28880         } catch (final IllegalArgumentException ignore) {
28881         }
28882         try {
28883             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28884                     measurements, new Matrix(1, 3), this);
28885             fail("IllegalArgumentException expected but not thrown");
28886         } catch (final IllegalArgumentException ignore) {
28887         }
28888         assertNull(calibrator);
28889     }
28890 
28891     @Test
28892     public void testConstructor223() throws WrongSizeException {
28893         final Matrix ba = generateBa();
28894         final double[] bias = ba.getBuffer();
28895         final double biasX = ba.getElementAtIndex(0);
28896         final double biasY = ba.getElementAtIndex(1);
28897         final double biasZ = ba.getElementAtIndex(2);
28898 
28899         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28900         final double latitude = Math.toRadians(
28901                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28902         final double longitude = Math.toRadians(
28903                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28904         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28905         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28906         final NEDVelocity nedVelocity = new NEDVelocity();
28907         final ECEFPosition ecefPosition = new ECEFPosition();
28908         final ECEFVelocity ecefVelocity = new ECEFVelocity();
28909         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28910                 ecefPosition, ecefVelocity);
28911 
28912         KnownPositionAccelerometerCalibrator calibrator =
28913                 new KnownPositionAccelerometerCalibrator(nedPosition,
28914                         true, ba);
28915 
28916         // check default values
28917         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28918         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28919         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28920         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28921         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28922         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28923         final Acceleration bx2 = new Acceleration(0.0,
28924                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28925         calibrator.getInitialBiasXAsAcceleration(bx2);
28926         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28927         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28928         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28929         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28930         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28931         final Acceleration by2 = new Acceleration(0.0,
28932                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28933         calibrator.getInitialBiasYAsAcceleration(by2);
28934         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28935         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28936         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28937         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28938         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28939         final Acceleration bz2 = new Acceleration(0.0,
28940                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28941         calibrator.getInitialBiasZAsAcceleration(bz2);
28942         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28943         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28944         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28945         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28946         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28947         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28948         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28949         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28950         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28951         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28952         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28953         final double[] bias1 = calibrator.getInitialBias();
28954         assertArrayEquals(bias1, bias, 0.0);
28955         final double[] bias2 = new double[3];
28956         calibrator.getInitialBias(bias2);
28957         assertArrayEquals(bias1, bias2, 0.0);
28958         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28959         assertEquals(b1, ba);
28960         final Matrix b2 = new Matrix(3, 1);
28961         calibrator.getInitialBiasAsMatrix(b2);
28962         assertEquals(b1, b2);
28963         final Matrix ma1 = calibrator.getInitialMa();
28964         assertEquals(ma1, new Matrix(3, 3));
28965         final Matrix ma2 = new Matrix(3, 3);
28966         calibrator.getInitialMa(ma2);
28967         assertEquals(ma1, ma2);
28968         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28969         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28970         final NEDPosition nedPosition1 = new NEDPosition();
28971         assertTrue(calibrator.getNedPosition(nedPosition1));
28972         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28973         assertNull(calibrator.getMeasurements());
28974         assertTrue(calibrator.isCommonAxisUsed());
28975         assertNull(calibrator.getListener());
28976         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28977         assertFalse(calibrator.isReady());
28978         assertFalse(calibrator.isRunning());
28979         assertNull(calibrator.getEstimatedBiases());
28980         assertFalse(calibrator.getEstimatedBiases(null));
28981         assertNull(calibrator.getEstimatedBiasesAsMatrix());
28982         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28983         assertNull(calibrator.getEstimatedBiasFx());
28984         assertNull(calibrator.getEstimatedBiasFy());
28985         assertNull(calibrator.getEstimatedBiasFz());
28986         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28987         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28988         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28989         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28990         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28991         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28992         assertNull(calibrator.getEstimatedMa());
28993         assertNull(calibrator.getEstimatedSx());
28994         assertNull(calibrator.getEstimatedSy());
28995         assertNull(calibrator.getEstimatedSz());
28996         assertNull(calibrator.getEstimatedMxy());
28997         assertNull(calibrator.getEstimatedMxz());
28998         assertNull(calibrator.getEstimatedMyx());
28999         assertNull(calibrator.getEstimatedMyz());
29000         assertNull(calibrator.getEstimatedMzx());
29001         assertNull(calibrator.getEstimatedMzy());
29002         assertNull(calibrator.getEstimatedCovariance());
29003         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29004         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29005                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29006         assertNotNull(calibrator.getGroundTruthGravityNorm());
29007         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29008         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29009         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29010                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29011         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29012         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29013         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29014 
29015         // Force IllegalArgumentException
29016         calibrator = null;
29017         try {
29018             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29019                     true, new Matrix(1, 1));
29020             fail("IllegalArgumentException expected but not thrown");
29021         } catch (final IllegalArgumentException ignore) {
29022         }
29023         try {
29024             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29025                     true, new Matrix(1, 3));
29026             fail("IllegalArgumentException expected but not thrown");
29027         } catch (final IllegalArgumentException ignore) {
29028         }
29029         assertNull(calibrator);
29030     }
29031 
29032     @Test
29033     public void testConstructor224() throws WrongSizeException {
29034         final Matrix ba = generateBa();
29035         final double[] bias = ba.getBuffer();
29036         final double biasX = ba.getElementAtIndex(0);
29037         final double biasY = ba.getElementAtIndex(1);
29038         final double biasZ = ba.getElementAtIndex(2);
29039 
29040         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29041         final double latitude = Math.toRadians(
29042                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29043         final double longitude = Math.toRadians(
29044                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29045         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29046         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29047         final NEDVelocity nedVelocity = new NEDVelocity();
29048         final ECEFPosition ecefPosition = new ECEFPosition();
29049         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29050         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29051                 ecefPosition, ecefVelocity);
29052 
29053         KnownPositionAccelerometerCalibrator calibrator =
29054                 new KnownPositionAccelerometerCalibrator(nedPosition,
29055                         true, ba, this);
29056 
29057         // check default values
29058         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29059         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29060         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29061         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29062         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29063         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29064         final Acceleration bx2 = new Acceleration(0.0,
29065                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29066         calibrator.getInitialBiasXAsAcceleration(bx2);
29067         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29068         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29069         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29070         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29071         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29072         final Acceleration by2 = new Acceleration(0.0,
29073                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29074         calibrator.getInitialBiasYAsAcceleration(by2);
29075         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29076         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29077         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29078         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29079         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29080         final Acceleration bz2 = new Acceleration(0.0,
29081                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29082         calibrator.getInitialBiasZAsAcceleration(bz2);
29083         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29084         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29085         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29086         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29087         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29088         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29089         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29090         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29091         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29092         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29093         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29094         final double[] bias1 = calibrator.getInitialBias();
29095         assertArrayEquals(bias1, bias, 0.0);
29096         final double[] bias2 = new double[3];
29097         calibrator.getInitialBias(bias2);
29098         assertArrayEquals(bias1, bias2, 0.0);
29099         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29100         assertEquals(b1, ba);
29101         final Matrix b2 = new Matrix(3, 1);
29102         calibrator.getInitialBiasAsMatrix(b2);
29103         assertEquals(b1, b2);
29104         final Matrix ma1 = calibrator.getInitialMa();
29105         assertEquals(ma1, new Matrix(3, 3));
29106         final Matrix ma2 = new Matrix(3, 3);
29107         calibrator.getInitialMa(ma2);
29108         assertEquals(ma1, ma2);
29109         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29110         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29111         final NEDPosition nedPosition1 = new NEDPosition();
29112         assertTrue(calibrator.getNedPosition(nedPosition1));
29113         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29114         assertNull(calibrator.getMeasurements());
29115         assertTrue(calibrator.isCommonAxisUsed());
29116         assertSame(calibrator.getListener(), this);
29117         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29118         assertFalse(calibrator.isReady());
29119         assertFalse(calibrator.isRunning());
29120         assertNull(calibrator.getEstimatedBiases());
29121         assertFalse(calibrator.getEstimatedBiases(null));
29122         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29123         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29124         assertNull(calibrator.getEstimatedBiasFx());
29125         assertNull(calibrator.getEstimatedBiasFy());
29126         assertNull(calibrator.getEstimatedBiasFz());
29127         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29128         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29129         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29130         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29131         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29132         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29133         assertNull(calibrator.getEstimatedMa());
29134         assertNull(calibrator.getEstimatedSx());
29135         assertNull(calibrator.getEstimatedSy());
29136         assertNull(calibrator.getEstimatedSz());
29137         assertNull(calibrator.getEstimatedMxy());
29138         assertNull(calibrator.getEstimatedMxz());
29139         assertNull(calibrator.getEstimatedMyx());
29140         assertNull(calibrator.getEstimatedMyz());
29141         assertNull(calibrator.getEstimatedMzx());
29142         assertNull(calibrator.getEstimatedMzy());
29143         assertNull(calibrator.getEstimatedCovariance());
29144         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29145         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29146                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29147         assertNotNull(calibrator.getGroundTruthGravityNorm());
29148         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29149         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29150         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29151                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29152         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29153         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29154         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29155 
29156         // Force IllegalArgumentException
29157         calibrator = null;
29158         try {
29159             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29160                     true, new Matrix(1, 1),
29161                     this);
29162             fail("IllegalArgumentException expected but not thrown");
29163         } catch (final IllegalArgumentException ignore) {
29164         }
29165         try {
29166             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29167                     true, new Matrix(1, 3),
29168                     this);
29169             fail("IllegalArgumentException expected but not thrown");
29170         } catch (final IllegalArgumentException ignore) {
29171         }
29172         assertNull(calibrator);
29173     }
29174 
29175     @Test
29176     public void testConstructor225() throws WrongSizeException {
29177         final Collection<StandardDeviationBodyKinematics> measurements =
29178                 Collections.emptyList();
29179 
29180         final Matrix ba = generateBa();
29181         final double[] bias = ba.getBuffer();
29182         final double biasX = ba.getElementAtIndex(0);
29183         final double biasY = ba.getElementAtIndex(1);
29184         final double biasZ = ba.getElementAtIndex(2);
29185 
29186         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29187         final double latitude = Math.toRadians(
29188                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29189         final double longitude = Math.toRadians(
29190                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29191         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29192         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29193         final NEDVelocity nedVelocity = new NEDVelocity();
29194         final ECEFPosition ecefPosition = new ECEFPosition();
29195         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29196         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29197                 ecefPosition, ecefVelocity);
29198 
29199         KnownPositionAccelerometerCalibrator calibrator =
29200                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
29201                         true, ba);
29202 
29203         // check default values
29204         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29205         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29206         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29207         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29208         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29209         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29210         final Acceleration bx2 = new Acceleration(0.0,
29211                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29212         calibrator.getInitialBiasXAsAcceleration(bx2);
29213         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29214         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29215         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29216         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29217         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29218         final Acceleration by2 = new Acceleration(0.0,
29219                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29220         calibrator.getInitialBiasYAsAcceleration(by2);
29221         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29222         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29223         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29224         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29225         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29226         final Acceleration bz2 = new Acceleration(0.0,
29227                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29228         calibrator.getInitialBiasZAsAcceleration(bz2);
29229         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29230         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29231         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29232         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29233         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29234         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29235         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29236         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29237         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29238         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29239         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29240         final double[] bias1 = calibrator.getInitialBias();
29241         assertArrayEquals(bias1, bias, 0.0);
29242         final double[] bias2 = new double[3];
29243         calibrator.getInitialBias(bias2);
29244         assertArrayEquals(bias1, bias2, 0.0);
29245         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29246         assertEquals(b1, ba);
29247         final Matrix b2 = new Matrix(3, 1);
29248         calibrator.getInitialBiasAsMatrix(b2);
29249         assertEquals(b1, b2);
29250         final Matrix ma1 = calibrator.getInitialMa();
29251         assertEquals(ma1, new Matrix(3, 3));
29252         final Matrix ma2 = new Matrix(3, 3);
29253         calibrator.getInitialMa(ma2);
29254         assertEquals(ma1, ma2);
29255         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29256         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29257         final NEDPosition nedPosition1 = new NEDPosition();
29258         assertTrue(calibrator.getNedPosition(nedPosition1));
29259         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29260         assertSame(calibrator.getMeasurements(), measurements);
29261         assertTrue(calibrator.isCommonAxisUsed());
29262         assertNull(calibrator.getListener());
29263         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29264         assertFalse(calibrator.isReady());
29265         assertFalse(calibrator.isRunning());
29266         assertNull(calibrator.getEstimatedBiases());
29267         assertFalse(calibrator.getEstimatedBiases(null));
29268         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29269         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29270         assertNull(calibrator.getEstimatedBiasFx());
29271         assertNull(calibrator.getEstimatedBiasFy());
29272         assertNull(calibrator.getEstimatedBiasFz());
29273         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29274         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29275         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29276         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29277         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29278         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29279         assertNull(calibrator.getEstimatedMa());
29280         assertNull(calibrator.getEstimatedSx());
29281         assertNull(calibrator.getEstimatedSy());
29282         assertNull(calibrator.getEstimatedSz());
29283         assertNull(calibrator.getEstimatedMxy());
29284         assertNull(calibrator.getEstimatedMxz());
29285         assertNull(calibrator.getEstimatedMyx());
29286         assertNull(calibrator.getEstimatedMyz());
29287         assertNull(calibrator.getEstimatedMzx());
29288         assertNull(calibrator.getEstimatedMzy());
29289         assertNull(calibrator.getEstimatedCovariance());
29290         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29291         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29292                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29293         assertNotNull(calibrator.getGroundTruthGravityNorm());
29294         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29295         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29296         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29297                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29298         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29299         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29300         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29301 
29302         // Force IllegalArgumentException
29303         calibrator = null;
29304         try {
29305             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29306                     measurements, true,
29307                     new Matrix(1, 1));
29308             fail("IllegalArgumentException expected but not thrown");
29309         } catch (final IllegalArgumentException ignore) {
29310         }
29311         try {
29312             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29313                     measurements, true,
29314                     new Matrix(1, 3));
29315             fail("IllegalArgumentException expected but not thrown");
29316         } catch (final IllegalArgumentException ignore) {
29317         }
29318         assertNull(calibrator);
29319     }
29320 
29321     @Test
29322     public void testConstructor226() throws WrongSizeException {
29323         final Collection<StandardDeviationBodyKinematics> measurements =
29324                 Collections.emptyList();
29325 
29326         final Matrix ba = generateBa();
29327         final double[] bias = ba.getBuffer();
29328         final double biasX = ba.getElementAtIndex(0);
29329         final double biasY = ba.getElementAtIndex(1);
29330         final double biasZ = ba.getElementAtIndex(2);
29331 
29332         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29333         final double latitude = Math.toRadians(
29334                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29335         final double longitude = Math.toRadians(
29336                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29337         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29338         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29339         final NEDVelocity nedVelocity = new NEDVelocity();
29340         final ECEFPosition ecefPosition = new ECEFPosition();
29341         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29342         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29343                 ecefPosition, ecefVelocity);
29344 
29345         KnownPositionAccelerometerCalibrator calibrator =
29346                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
29347                         true, ba, this);
29348 
29349         // check default values
29350         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29351         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29352         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29353         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29354         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29355         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29356         final Acceleration bx2 = new Acceleration(0.0,
29357                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29358         calibrator.getInitialBiasXAsAcceleration(bx2);
29359         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29360         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29361         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29362         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29363         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29364         final Acceleration by2 = new Acceleration(0.0,
29365                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29366         calibrator.getInitialBiasYAsAcceleration(by2);
29367         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29368         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29369         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29370         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29371         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29372         final Acceleration bz2 = new Acceleration(0.0,
29373                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29374         calibrator.getInitialBiasZAsAcceleration(bz2);
29375         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29376         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29377         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29378         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29379         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29380         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29381         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29382         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29383         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29384         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29385         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29386         final double[] bias1 = calibrator.getInitialBias();
29387         assertArrayEquals(bias1, bias, 0.0);
29388         final double[] bias2 = new double[3];
29389         calibrator.getInitialBias(bias2);
29390         assertArrayEquals(bias1, bias2, 0.0);
29391         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29392         assertEquals(b1, ba);
29393         final Matrix b2 = new Matrix(3, 1);
29394         calibrator.getInitialBiasAsMatrix(b2);
29395         assertEquals(b1, b2);
29396         final Matrix ma1 = calibrator.getInitialMa();
29397         assertEquals(ma1, new Matrix(3, 3));
29398         final Matrix ma2 = new Matrix(3, 3);
29399         calibrator.getInitialMa(ma2);
29400         assertEquals(ma1, ma2);
29401         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29402         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29403         final NEDPosition nedPosition1 = new NEDPosition();
29404         assertTrue(calibrator.getNedPosition(nedPosition1));
29405         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29406         assertSame(calibrator.getMeasurements(), measurements);
29407         assertTrue(calibrator.isCommonAxisUsed());
29408         assertSame(calibrator.getListener(), this);
29409         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29410         assertFalse(calibrator.isReady());
29411         assertFalse(calibrator.isRunning());
29412         assertNull(calibrator.getEstimatedBiases());
29413         assertFalse(calibrator.getEstimatedBiases(null));
29414         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29415         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29416         assertNull(calibrator.getEstimatedBiasFx());
29417         assertNull(calibrator.getEstimatedBiasFy());
29418         assertNull(calibrator.getEstimatedBiasFz());
29419         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29420         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29421         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29422         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29423         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29424         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29425         assertNull(calibrator.getEstimatedMa());
29426         assertNull(calibrator.getEstimatedSx());
29427         assertNull(calibrator.getEstimatedSy());
29428         assertNull(calibrator.getEstimatedSz());
29429         assertNull(calibrator.getEstimatedMxy());
29430         assertNull(calibrator.getEstimatedMxz());
29431         assertNull(calibrator.getEstimatedMyx());
29432         assertNull(calibrator.getEstimatedMyz());
29433         assertNull(calibrator.getEstimatedMzx());
29434         assertNull(calibrator.getEstimatedMzy());
29435         assertNull(calibrator.getEstimatedCovariance());
29436         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29437         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29438                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29439         assertNotNull(calibrator.getGroundTruthGravityNorm());
29440         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29441         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29442         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29443                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29444         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29445         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29446         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29447 
29448         // Force IllegalArgumentException
29449         calibrator = null;
29450         try {
29451             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29452                     measurements, true,
29453                     new Matrix(1, 1), this);
29454             fail("IllegalArgumentException expected but not thrown");
29455         } catch (final IllegalArgumentException ignore) {
29456         }
29457         try {
29458             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29459                     measurements, true,
29460                     new Matrix(1, 3), this);
29461             fail("IllegalArgumentException expected but not thrown");
29462         } catch (final IllegalArgumentException ignore) {
29463         }
29464         assertNull(calibrator);
29465     }
29466 
29467     @Test
29468     public void testConstructor227() throws WrongSizeException {
29469         final Matrix ba = generateBa();
29470         final double[] bias = ba.getBuffer();
29471         final double biasX = ba.getElementAtIndex(0);
29472         final double biasY = ba.getElementAtIndex(1);
29473         final double biasZ = ba.getElementAtIndex(2);
29474 
29475         final Matrix ma = generateMaCommonAxis();
29476         final double sx = ma.getElementAt(0, 0);
29477         final double sy = ma.getElementAt(1, 1);
29478         final double sz = ma.getElementAt(2, 2);
29479         final double mxy = ma.getElementAt(0, 1);
29480         final double mxz = ma.getElementAt(0, 2);
29481         final double myx = ma.getElementAt(1, 0);
29482         final double myz = ma.getElementAt(1, 2);
29483         final double mzx = ma.getElementAt(2, 0);
29484         final double mzy = ma.getElementAt(2, 1);
29485 
29486         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29487         final double latitude = Math.toRadians(
29488                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29489         final double longitude = Math.toRadians(
29490                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29491         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29492         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29493         final NEDVelocity nedVelocity = new NEDVelocity();
29494         final ECEFPosition ecefPosition = new ECEFPosition();
29495         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29496         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29497                 ecefPosition, ecefVelocity);
29498 
29499         KnownPositionAccelerometerCalibrator calibrator =
29500                 new KnownPositionAccelerometerCalibrator(nedPosition, ba, ma);
29501 
29502         // check default values
29503         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29504         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29505         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29506         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29507         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29508         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29509         final Acceleration bx2 = new Acceleration(0.0,
29510                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29511         calibrator.getInitialBiasXAsAcceleration(bx2);
29512         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29513         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29514         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29515         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29516         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29517         final Acceleration by2 = new Acceleration(0.0,
29518                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29519         calibrator.getInitialBiasYAsAcceleration(by2);
29520         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29521         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29522         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29523         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29524         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29525         final Acceleration bz2 = new Acceleration(0.0,
29526                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29527         calibrator.getInitialBiasZAsAcceleration(bz2);
29528         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29529         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29530         assertEquals(calibrator.getInitialSx(), sx, 0.0);
29531         assertEquals(calibrator.getInitialSy(), sy, 0.0);
29532         assertEquals(calibrator.getInitialSz(), sz, 0.0);
29533         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29534         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29535         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29536         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29537         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29538         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29539         final double[] bias1 = calibrator.getInitialBias();
29540         assertArrayEquals(bias1, bias, 0.0);
29541         final double[] bias2 = new double[3];
29542         calibrator.getInitialBias(bias2);
29543         assertArrayEquals(bias1, bias2, 0.0);
29544         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29545         assertEquals(b1, ba);
29546         final Matrix b2 = new Matrix(3, 1);
29547         calibrator.getInitialBiasAsMatrix(b2);
29548         assertEquals(b1, b2);
29549         final Matrix ma1 = new Matrix(3, 3);
29550         ma1.setSubmatrix(0, 0,
29551                 2, 2,
29552                 new double[]{sx, myx, mzx,
29553                         mxy, sy, mzy,
29554                         mxz, myz, sz});
29555         assertEquals(calibrator.getInitialMa(), ma1);
29556         final Matrix ma2 = new Matrix(3, 3);
29557         calibrator.getInitialMa(ma2);
29558         assertEquals(ma1, ma2);
29559         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29560         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29561         final NEDPosition nedPosition1 = new NEDPosition();
29562         assertTrue(calibrator.getNedPosition(nedPosition1));
29563         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29564         assertNull(calibrator.getMeasurements());
29565         assertFalse(calibrator.isCommonAxisUsed());
29566         assertNull(calibrator.getListener());
29567         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29568         assertFalse(calibrator.isReady());
29569         assertFalse(calibrator.isRunning());
29570         assertNull(calibrator.getEstimatedBiases());
29571         assertFalse(calibrator.getEstimatedBiases(null));
29572         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29573         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29574         assertNull(calibrator.getEstimatedBiasFx());
29575         assertNull(calibrator.getEstimatedBiasFy());
29576         assertNull(calibrator.getEstimatedBiasFz());
29577         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29578         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29579         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29580         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29581         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29582         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29583         assertNull(calibrator.getEstimatedMa());
29584         assertNull(calibrator.getEstimatedSx());
29585         assertNull(calibrator.getEstimatedSy());
29586         assertNull(calibrator.getEstimatedSz());
29587         assertNull(calibrator.getEstimatedMxy());
29588         assertNull(calibrator.getEstimatedMxz());
29589         assertNull(calibrator.getEstimatedMyx());
29590         assertNull(calibrator.getEstimatedMyz());
29591         assertNull(calibrator.getEstimatedMzx());
29592         assertNull(calibrator.getEstimatedMzy());
29593         assertNull(calibrator.getEstimatedCovariance());
29594         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29595         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29596                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29597         assertNotNull(calibrator.getGroundTruthGravityNorm());
29598         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29599         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29600         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29601                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29602         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29603         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29604         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29605 
29606         // Force IllegalArgumentException
29607         calibrator = null;
29608         try {
29609             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29610                     new Matrix(1, 1), ma);
29611             fail("IllegalArgumentException expected but not thrown");
29612         } catch (final IllegalArgumentException ignore) {
29613         }
29614         try {
29615             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29616                     new Matrix(1, 3), ma);
29617             fail("IllegalArgumentException expected but not thrown");
29618         } catch (final IllegalArgumentException ignore) {
29619         }
29620         try {
29621             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29622                     ba, new Matrix(1, 3));
29623             fail("IllegalArgumentException expected but not thrown");
29624         } catch (final IllegalArgumentException ignore) {
29625         }
29626         try {
29627             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29628                     ba, new Matrix(3, 1));
29629             fail("IllegalArgumentException expected but not thrown");
29630         } catch (final IllegalArgumentException ignore) {
29631         }
29632         assertNull(calibrator);
29633     }
29634 
29635     @Test
29636     public void testConstructor228() throws WrongSizeException {
29637         final Matrix ba = generateBa();
29638         final double[] bias = ba.getBuffer();
29639         final double biasX = ba.getElementAtIndex(0);
29640         final double biasY = ba.getElementAtIndex(1);
29641         final double biasZ = ba.getElementAtIndex(2);
29642 
29643         final Matrix ma = generateMaCommonAxis();
29644         final double sx = ma.getElementAt(0, 0);
29645         final double sy = ma.getElementAt(1, 1);
29646         final double sz = ma.getElementAt(2, 2);
29647         final double mxy = ma.getElementAt(0, 1);
29648         final double mxz = ma.getElementAt(0, 2);
29649         final double myx = ma.getElementAt(1, 0);
29650         final double myz = ma.getElementAt(1, 2);
29651         final double mzx = ma.getElementAt(2, 0);
29652         final double mzy = ma.getElementAt(2, 1);
29653 
29654         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29655         final double latitude = Math.toRadians(
29656                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29657         final double longitude = Math.toRadians(
29658                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29659         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29660         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29661         final NEDVelocity nedVelocity = new NEDVelocity();
29662         final ECEFPosition ecefPosition = new ECEFPosition();
29663         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29664         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29665                 ecefPosition, ecefVelocity);
29666 
29667         KnownPositionAccelerometerCalibrator calibrator =
29668                 new KnownPositionAccelerometerCalibrator(nedPosition,
29669                         ba, ma, this);
29670 
29671         // check default values
29672         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29673         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29674         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29675         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29676         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29677         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29678         final Acceleration bx2 = new Acceleration(0.0,
29679                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29680         calibrator.getInitialBiasXAsAcceleration(bx2);
29681         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29682         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29683         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29684         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29685         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29686         final Acceleration by2 = new Acceleration(0.0,
29687                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29688         calibrator.getInitialBiasYAsAcceleration(by2);
29689         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29690         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29691         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29692         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29693         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29694         final Acceleration bz2 = new Acceleration(0.0,
29695                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29696         calibrator.getInitialBiasZAsAcceleration(bz2);
29697         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29698         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29699         assertEquals(calibrator.getInitialSx(), sx, 0.0);
29700         assertEquals(calibrator.getInitialSy(), sy, 0.0);
29701         assertEquals(calibrator.getInitialSz(), sz, 0.0);
29702         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29703         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29704         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29705         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29706         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29707         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29708         final double[] bias1 = calibrator.getInitialBias();
29709         assertArrayEquals(bias1, bias, 0.0);
29710         final double[] bias2 = new double[3];
29711         calibrator.getInitialBias(bias2);
29712         assertArrayEquals(bias1, bias2, 0.0);
29713         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29714         assertEquals(b1, ba);
29715         final Matrix b2 = new Matrix(3, 1);
29716         calibrator.getInitialBiasAsMatrix(b2);
29717         assertEquals(b1, b2);
29718         final Matrix ma1 = new Matrix(3, 3);
29719         ma1.setSubmatrix(0, 0,
29720                 2, 2,
29721                 new double[]{sx, myx, mzx,
29722                         mxy, sy, mzy,
29723                         mxz, myz, sz});
29724         assertEquals(calibrator.getInitialMa(), ma1);
29725         final Matrix ma2 = new Matrix(3, 3);
29726         calibrator.getInitialMa(ma2);
29727         assertEquals(ma1, ma2);
29728         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29729         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29730         final NEDPosition nedPosition1 = new NEDPosition();
29731         assertTrue(calibrator.getNedPosition(nedPosition1));
29732         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29733         assertNull(calibrator.getMeasurements());
29734         assertFalse(calibrator.isCommonAxisUsed());
29735         assertSame(calibrator.getListener(), this);
29736         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29737         assertFalse(calibrator.isReady());
29738         assertFalse(calibrator.isRunning());
29739         assertNull(calibrator.getEstimatedBiases());
29740         assertFalse(calibrator.getEstimatedBiases(null));
29741         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29742         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29743         assertNull(calibrator.getEstimatedBiasFx());
29744         assertNull(calibrator.getEstimatedBiasFy());
29745         assertNull(calibrator.getEstimatedBiasFz());
29746         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29747         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29748         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29749         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29750         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29751         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29752         assertNull(calibrator.getEstimatedMa());
29753         assertNull(calibrator.getEstimatedSx());
29754         assertNull(calibrator.getEstimatedSy());
29755         assertNull(calibrator.getEstimatedSz());
29756         assertNull(calibrator.getEstimatedMxy());
29757         assertNull(calibrator.getEstimatedMxz());
29758         assertNull(calibrator.getEstimatedMyx());
29759         assertNull(calibrator.getEstimatedMyz());
29760         assertNull(calibrator.getEstimatedMzx());
29761         assertNull(calibrator.getEstimatedMzy());
29762         assertNull(calibrator.getEstimatedCovariance());
29763         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29764         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29765                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29766         assertNotNull(calibrator.getGroundTruthGravityNorm());
29767         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29768         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29769         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29770                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29771         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29772         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29773         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29774 
29775         // Force IllegalArgumentException
29776         calibrator = null;
29777         try {
29778             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29779                     new Matrix(1, 1), ma, this);
29780             fail("IllegalArgumentException expected but not thrown");
29781         } catch (final IllegalArgumentException ignore) {
29782         }
29783         try {
29784             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29785                     new Matrix(1, 3), ma, this);
29786             fail("IllegalArgumentException expected but not thrown");
29787         } catch (final IllegalArgumentException ignore) {
29788         }
29789         try {
29790             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29791                     ba, new Matrix(1, 3), this);
29792             fail("IllegalArgumentException expected but not thrown");
29793         } catch (final IllegalArgumentException ignore) {
29794         }
29795         try {
29796             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29797                     ba, new Matrix(3, 1), this);
29798             fail("IllegalArgumentException expected but not thrown");
29799         } catch (final IllegalArgumentException ignore) {
29800         }
29801         assertNull(calibrator);
29802     }
29803 
29804     @Test
29805     public void testConstructor229() throws WrongSizeException {
29806         final Collection<StandardDeviationBodyKinematics> measurements =
29807                 Collections.emptyList();
29808 
29809         final Matrix ba = generateBa();
29810         final double[] bias = ba.getBuffer();
29811         final double biasX = ba.getElementAtIndex(0);
29812         final double biasY = ba.getElementAtIndex(1);
29813         final double biasZ = ba.getElementAtIndex(2);
29814 
29815         final Matrix ma = generateMaCommonAxis();
29816         final double sx = ma.getElementAt(0, 0);
29817         final double sy = ma.getElementAt(1, 1);
29818         final double sz = ma.getElementAt(2, 2);
29819         final double mxy = ma.getElementAt(0, 1);
29820         final double mxz = ma.getElementAt(0, 2);
29821         final double myx = ma.getElementAt(1, 0);
29822         final double myz = ma.getElementAt(1, 2);
29823         final double mzx = ma.getElementAt(2, 0);
29824         final double mzy = ma.getElementAt(2, 1);
29825 
29826         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29827         final double latitude = Math.toRadians(
29828                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29829         final double longitude = Math.toRadians(
29830                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29831         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29832         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29833         final NEDVelocity nedVelocity = new NEDVelocity();
29834         final ECEFPosition ecefPosition = new ECEFPosition();
29835         final ECEFVelocity ecefVelocity = new ECEFVelocity();
29836         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29837                 ecefPosition, ecefVelocity);
29838 
29839         KnownPositionAccelerometerCalibrator calibrator =
29840                 new KnownPositionAccelerometerCalibrator(nedPosition,
29841                         measurements, ba, ma);
29842 
29843         // check default values
29844         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29845         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29846         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29847         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29848         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29849         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29850         final Acceleration bx2 = new Acceleration(0.0,
29851                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29852         calibrator.getInitialBiasXAsAcceleration(bx2);
29853         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29854         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29855         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29856         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29857         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29858         final Acceleration by2 = new Acceleration(0.0,
29859                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29860         calibrator.getInitialBiasYAsAcceleration(by2);
29861         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29862         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29863         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29864         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29865         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29866         final Acceleration bz2 = new Acceleration(0.0,
29867                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29868         calibrator.getInitialBiasZAsAcceleration(bz2);
29869         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29870         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29871         assertEquals(calibrator.getInitialSx(), sx, 0.0);
29872         assertEquals(calibrator.getInitialSy(), sy, 0.0);
29873         assertEquals(calibrator.getInitialSz(), sz, 0.0);
29874         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29875         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29876         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29877         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29878         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29879         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29880         final double[] bias1 = calibrator.getInitialBias();
29881         assertArrayEquals(bias1, bias, 0.0);
29882         final double[] bias2 = new double[3];
29883         calibrator.getInitialBias(bias2);
29884         assertArrayEquals(bias1, bias2, 0.0);
29885         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29886         assertEquals(b1, ba);
29887         final Matrix b2 = new Matrix(3, 1);
29888         calibrator.getInitialBiasAsMatrix(b2);
29889         assertEquals(b1, b2);
29890         final Matrix ma1 = new Matrix(3, 3);
29891         ma1.setSubmatrix(0, 0,
29892                 2, 2,
29893                 new double[]{sx, myx, mzx,
29894                         mxy, sy, mzy,
29895                         mxz, myz, sz});
29896         assertEquals(calibrator.getInitialMa(), ma1);
29897         final Matrix ma2 = new Matrix(3, 3);
29898         calibrator.getInitialMa(ma2);
29899         assertEquals(ma1, ma2);
29900         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29901         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29902         final NEDPosition nedPosition1 = new NEDPosition();
29903         assertTrue(calibrator.getNedPosition(nedPosition1));
29904         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29905         assertSame(calibrator.getMeasurements(), measurements);
29906         assertFalse(calibrator.isCommonAxisUsed());
29907         assertNull(calibrator.getListener());
29908         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29909         assertFalse(calibrator.isReady());
29910         assertFalse(calibrator.isRunning());
29911         assertNull(calibrator.getEstimatedBiases());
29912         assertFalse(calibrator.getEstimatedBiases(null));
29913         assertNull(calibrator.getEstimatedBiasesAsMatrix());
29914         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29915         assertNull(calibrator.getEstimatedBiasFx());
29916         assertNull(calibrator.getEstimatedBiasFy());
29917         assertNull(calibrator.getEstimatedBiasFz());
29918         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29919         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29920         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29921         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29922         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29923         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29924         assertNull(calibrator.getEstimatedMa());
29925         assertNull(calibrator.getEstimatedSx());
29926         assertNull(calibrator.getEstimatedSy());
29927         assertNull(calibrator.getEstimatedSz());
29928         assertNull(calibrator.getEstimatedMxy());
29929         assertNull(calibrator.getEstimatedMxz());
29930         assertNull(calibrator.getEstimatedMyx());
29931         assertNull(calibrator.getEstimatedMyz());
29932         assertNull(calibrator.getEstimatedMzx());
29933         assertNull(calibrator.getEstimatedMzy());
29934         assertNull(calibrator.getEstimatedCovariance());
29935         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29936         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29937                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29938         assertNotNull(calibrator.getGroundTruthGravityNorm());
29939         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29940         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29941         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29942                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29943         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29944         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29945         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29946 
29947         // Force IllegalArgumentException
29948         calibrator = null;
29949         try {
29950             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29951                     measurements, new Matrix(1, 1), ma);
29952             fail("IllegalArgumentException expected but not thrown");
29953         } catch (final IllegalArgumentException ignore) {
29954         }
29955         try {
29956             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29957                     measurements, new Matrix(1, 3), ma);
29958             fail("IllegalArgumentException expected but not thrown");
29959         } catch (final IllegalArgumentException ignore) {
29960         }
29961         try {
29962             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29963                     measurements, ba, new Matrix(1, 3));
29964             fail("IllegalArgumentException expected but not thrown");
29965         } catch (final IllegalArgumentException ignore) {
29966         }
29967         try {
29968             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29969                     measurements, ba, new Matrix(3, 1));
29970             fail("IllegalArgumentException expected but not thrown");
29971         } catch (final IllegalArgumentException ignore) {
29972         }
29973         assertNull(calibrator);
29974     }
29975 
29976     @Test
29977     public void testConstructor230() throws WrongSizeException {
29978         final Collection<StandardDeviationBodyKinematics> measurements =
29979                 Collections.emptyList();
29980 
29981         final Matrix ba = generateBa();
29982         final double[] bias = ba.getBuffer();
29983         final double biasX = ba.getElementAtIndex(0);
29984         final double biasY = ba.getElementAtIndex(1);
29985         final double biasZ = ba.getElementAtIndex(2);
29986 
29987         final Matrix ma = generateMaCommonAxis();
29988         final double sx = ma.getElementAt(0, 0);
29989         final double sy = ma.getElementAt(1, 1);
29990         final double sz = ma.getElementAt(2, 2);
29991         final double mxy = ma.getElementAt(0, 1);
29992         final double mxz = ma.getElementAt(0, 2);
29993         final double myx = ma.getElementAt(1, 0);
29994         final double myz = ma.getElementAt(1, 2);
29995         final double mzx = ma.getElementAt(2, 0);
29996         final double mzy = ma.getElementAt(2, 1);
29997 
29998         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29999         final double latitude = Math.toRadians(
30000                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30001         final double longitude = Math.toRadians(
30002                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30003         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30004         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30005         final NEDVelocity nedVelocity = new NEDVelocity();
30006         final ECEFPosition ecefPosition = new ECEFPosition();
30007         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30008         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30009                 ecefPosition, ecefVelocity);
30010 
30011         KnownPositionAccelerometerCalibrator calibrator =
30012                 new KnownPositionAccelerometerCalibrator(nedPosition,
30013                         measurements, ba, ma, this);
30014 
30015         // check default values
30016         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30017         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30018         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30019         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30020         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30021         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30022         final Acceleration bx2 = new Acceleration(0.0,
30023                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30024         calibrator.getInitialBiasXAsAcceleration(bx2);
30025         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30026         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30027         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30028         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30029         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30030         final Acceleration by2 = new Acceleration(0.0,
30031                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30032         calibrator.getInitialBiasYAsAcceleration(by2);
30033         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30034         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30035         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30036         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30037         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30038         final Acceleration bz2 = new Acceleration(0.0,
30039                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30040         calibrator.getInitialBiasZAsAcceleration(bz2);
30041         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30042         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30043         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30044         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30045         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30046         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30047         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30048         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30049         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30050         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30051         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30052         final double[] bias1 = calibrator.getInitialBias();
30053         assertArrayEquals(bias1, bias, 0.0);
30054         final double[] bias2 = new double[3];
30055         calibrator.getInitialBias(bias2);
30056         assertArrayEquals(bias1, bias2, 0.0);
30057         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30058         assertEquals(b1, ba);
30059         final Matrix b2 = new Matrix(3, 1);
30060         calibrator.getInitialBiasAsMatrix(b2);
30061         assertEquals(b1, b2);
30062         final Matrix ma1 = new Matrix(3, 3);
30063         ma1.setSubmatrix(0, 0,
30064                 2, 2,
30065                 new double[]{sx, myx, mzx,
30066                         mxy, sy, mzy,
30067                         mxz, myz, sz});
30068         assertEquals(calibrator.getInitialMa(), ma1);
30069         final Matrix ma2 = new Matrix(3, 3);
30070         calibrator.getInitialMa(ma2);
30071         assertEquals(ma1, ma2);
30072         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30073         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30074         final NEDPosition nedPosition1 = new NEDPosition();
30075         assertTrue(calibrator.getNedPosition(nedPosition1));
30076         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30077         assertSame(calibrator.getMeasurements(), measurements);
30078         assertFalse(calibrator.isCommonAxisUsed());
30079         assertSame(calibrator.getListener(), this);
30080         assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
30081         assertFalse(calibrator.isReady());
30082         assertFalse(calibrator.isRunning());
30083         assertNull(calibrator.getEstimatedBiases());
30084         assertFalse(calibrator.getEstimatedBiases(null));
30085         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30086         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30087         assertNull(calibrator.getEstimatedBiasFx());
30088         assertNull(calibrator.getEstimatedBiasFy());
30089         assertNull(calibrator.getEstimatedBiasFz());
30090         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30091         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30092         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30093         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30094         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30095         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30096         assertNull(calibrator.getEstimatedMa());
30097         assertNull(calibrator.getEstimatedSx());
30098         assertNull(calibrator.getEstimatedSy());
30099         assertNull(calibrator.getEstimatedSz());
30100         assertNull(calibrator.getEstimatedMxy());
30101         assertNull(calibrator.getEstimatedMxz());
30102         assertNull(calibrator.getEstimatedMyx());
30103         assertNull(calibrator.getEstimatedMyz());
30104         assertNull(calibrator.getEstimatedMzx());
30105         assertNull(calibrator.getEstimatedMzy());
30106         assertNull(calibrator.getEstimatedCovariance());
30107         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30108         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30109                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30110         assertNotNull(calibrator.getGroundTruthGravityNorm());
30111         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30112         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30113         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30114                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30115         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30116         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30117         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30118 
30119         // Force IllegalArgumentException
30120         calibrator = null;
30121         try {
30122             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30123                     measurements, new Matrix(1, 1), ma, this);
30124             fail("IllegalArgumentException expected but not thrown");
30125         } catch (final IllegalArgumentException ignore) {
30126         }
30127         try {
30128             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30129                     measurements, new Matrix(1, 3), ma, this);
30130             fail("IllegalArgumentException expected but not thrown");
30131         } catch (final IllegalArgumentException ignore) {
30132         }
30133         try {
30134             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30135                     measurements, ba, new Matrix(1, 3), this);
30136             fail("IllegalArgumentException expected but not thrown");
30137         } catch (final IllegalArgumentException ignore) {
30138         }
30139         try {
30140             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30141                     measurements, ba, new Matrix(3, 1), this);
30142             fail("IllegalArgumentException expected but not thrown");
30143         } catch (final IllegalArgumentException ignore) {
30144         }
30145         assertNull(calibrator);
30146     }
30147 
30148     @Test
30149     public void testConstructor231() throws WrongSizeException {
30150         final Matrix ba = generateBa();
30151         final double[] bias = ba.getBuffer();
30152         final double biasX = ba.getElementAtIndex(0);
30153         final double biasY = ba.getElementAtIndex(1);
30154         final double biasZ = ba.getElementAtIndex(2);
30155 
30156         final Matrix ma = generateMaCommonAxis();
30157         final double sx = ma.getElementAt(0, 0);
30158         final double sy = ma.getElementAt(1, 1);
30159         final double sz = ma.getElementAt(2, 2);
30160         final double mxy = ma.getElementAt(0, 1);
30161         final double mxz = ma.getElementAt(0, 2);
30162         final double myx = ma.getElementAt(1, 0);
30163         final double myz = ma.getElementAt(1, 2);
30164         final double mzx = ma.getElementAt(2, 0);
30165         final double mzy = ma.getElementAt(2, 1);
30166 
30167         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30168         final double latitude = Math.toRadians(
30169                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30170         final double longitude = Math.toRadians(
30171                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30172         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30173         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30174         final NEDVelocity nedVelocity = new NEDVelocity();
30175         final ECEFPosition ecefPosition = new ECEFPosition();
30176         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30177         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30178                 ecefPosition, ecefVelocity);
30179 
30180         KnownPositionAccelerometerCalibrator calibrator =
30181                 new KnownPositionAccelerometerCalibrator(nedPosition,
30182                         true, ba, ma);
30183 
30184         // check default values
30185         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30186         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30187         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30188         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30189         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30190         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30191         final Acceleration bx2 = new Acceleration(0.0,
30192                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30193         calibrator.getInitialBiasXAsAcceleration(bx2);
30194         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30195         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30196         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30197         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30198         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30199         final Acceleration by2 = new Acceleration(0.0,
30200                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30201         calibrator.getInitialBiasYAsAcceleration(by2);
30202         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30203         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30204         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30205         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30206         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30207         final Acceleration bz2 = new Acceleration(0.0,
30208                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30209         calibrator.getInitialBiasZAsAcceleration(bz2);
30210         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30211         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30212         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30213         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30214         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30215         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30216         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30217         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30218         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30219         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30220         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30221         final double[] bias1 = calibrator.getInitialBias();
30222         assertArrayEquals(bias1, bias, 0.0);
30223         final double[] bias2 = new double[3];
30224         calibrator.getInitialBias(bias2);
30225         assertArrayEquals(bias1, bias2, 0.0);
30226         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30227         assertEquals(b1, ba);
30228         final Matrix b2 = new Matrix(3, 1);
30229         calibrator.getInitialBiasAsMatrix(b2);
30230         assertEquals(b1, b2);
30231         final Matrix ma1 = new Matrix(3, 3);
30232         ma1.setSubmatrix(0, 0,
30233                 2, 2,
30234                 new double[]{sx, myx, mzx,
30235                         mxy, sy, mzy,
30236                         mxz, myz, sz});
30237         assertEquals(calibrator.getInitialMa(), ma1);
30238         final Matrix ma2 = new Matrix(3, 3);
30239         calibrator.getInitialMa(ma2);
30240         assertEquals(ma1, ma2);
30241         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30242         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30243         final NEDPosition nedPosition1 = new NEDPosition();
30244         assertTrue(calibrator.getNedPosition(nedPosition1));
30245         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30246         assertNull(calibrator.getMeasurements());
30247         assertTrue(calibrator.isCommonAxisUsed());
30248         assertNull(calibrator.getListener());
30249         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30250         assertFalse(calibrator.isReady());
30251         assertFalse(calibrator.isRunning());
30252         assertNull(calibrator.getEstimatedBiases());
30253         assertFalse(calibrator.getEstimatedBiases(null));
30254         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30255         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30256         assertNull(calibrator.getEstimatedBiasFx());
30257         assertNull(calibrator.getEstimatedBiasFy());
30258         assertNull(calibrator.getEstimatedBiasFz());
30259         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30260         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30261         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30262         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30263         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30264         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30265         assertNull(calibrator.getEstimatedMa());
30266         assertNull(calibrator.getEstimatedSx());
30267         assertNull(calibrator.getEstimatedSy());
30268         assertNull(calibrator.getEstimatedSz());
30269         assertNull(calibrator.getEstimatedMxy());
30270         assertNull(calibrator.getEstimatedMxz());
30271         assertNull(calibrator.getEstimatedMyx());
30272         assertNull(calibrator.getEstimatedMyz());
30273         assertNull(calibrator.getEstimatedMzx());
30274         assertNull(calibrator.getEstimatedMzy());
30275         assertNull(calibrator.getEstimatedCovariance());
30276         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30277         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30278                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30279         assertNotNull(calibrator.getGroundTruthGravityNorm());
30280         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30281         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30282         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30283                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30284         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30285         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30286         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30287 
30288         // Force IllegalArgumentException
30289         calibrator = null;
30290         try {
30291             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30292                     true, new Matrix(1, 1), ma);
30293             fail("IllegalArgumentException expected but not thrown");
30294         } catch (final IllegalArgumentException ignore) {
30295         }
30296         try {
30297             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30298                     true, new Matrix(1, 3), ma);
30299             fail("IllegalArgumentException expected but not thrown");
30300         } catch (final IllegalArgumentException ignore) {
30301         }
30302         try {
30303             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30304                     true, ba, new Matrix(1, 3));
30305             fail("IllegalArgumentException expected but not thrown");
30306         } catch (final IllegalArgumentException ignore) {
30307         }
30308         try {
30309             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30310                     true, ba, new Matrix(3, 1));
30311             fail("IllegalArgumentException expected but not thrown");
30312         } catch (final IllegalArgumentException ignore) {
30313         }
30314         assertNull(calibrator);
30315     }
30316 
30317     @Test
30318     public void testConstructor232() throws WrongSizeException {
30319         final Matrix ba = generateBa();
30320         final double[] bias = ba.getBuffer();
30321         final double biasX = ba.getElementAtIndex(0);
30322         final double biasY = ba.getElementAtIndex(1);
30323         final double biasZ = ba.getElementAtIndex(2);
30324 
30325         final Matrix ma = generateMaCommonAxis();
30326         final double sx = ma.getElementAt(0, 0);
30327         final double sy = ma.getElementAt(1, 1);
30328         final double sz = ma.getElementAt(2, 2);
30329         final double mxy = ma.getElementAt(0, 1);
30330         final double mxz = ma.getElementAt(0, 2);
30331         final double myx = ma.getElementAt(1, 0);
30332         final double myz = ma.getElementAt(1, 2);
30333         final double mzx = ma.getElementAt(2, 0);
30334         final double mzy = ma.getElementAt(2, 1);
30335 
30336         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30337         final double latitude = Math.toRadians(
30338                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30339         final double longitude = Math.toRadians(
30340                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30341         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30342         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30343         final NEDVelocity nedVelocity = new NEDVelocity();
30344         final ECEFPosition ecefPosition = new ECEFPosition();
30345         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30346         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30347                 ecefPosition, ecefVelocity);
30348 
30349         KnownPositionAccelerometerCalibrator calibrator =
30350                 new KnownPositionAccelerometerCalibrator(nedPosition,
30351                         true, ba, ma, this);
30352 
30353         // check default values
30354         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30355         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30356         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30357         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30358         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30359         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30360         final Acceleration bx2 = new Acceleration(0.0,
30361                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30362         calibrator.getInitialBiasXAsAcceleration(bx2);
30363         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30364         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30365         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30366         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30367         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30368         final Acceleration by2 = new Acceleration(0.0,
30369                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30370         calibrator.getInitialBiasYAsAcceleration(by2);
30371         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30372         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30373         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30374         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30375         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30376         final Acceleration bz2 = new Acceleration(0.0,
30377                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30378         calibrator.getInitialBiasZAsAcceleration(bz2);
30379         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30380         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30381         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30382         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30383         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30384         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30385         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30386         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30387         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30388         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30389         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30390         final double[] bias1 = calibrator.getInitialBias();
30391         assertArrayEquals(bias1, bias, 0.0);
30392         final double[] bias2 = new double[3];
30393         calibrator.getInitialBias(bias2);
30394         assertArrayEquals(bias1, bias2, 0.0);
30395         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30396         assertEquals(b1, ba);
30397         final Matrix b2 = new Matrix(3, 1);
30398         calibrator.getInitialBiasAsMatrix(b2);
30399         assertEquals(b1, b2);
30400         final Matrix ma1 = new Matrix(3, 3);
30401         ma1.setSubmatrix(0, 0,
30402                 2, 2,
30403                 new double[]{sx, myx, mzx,
30404                         mxy, sy, mzy,
30405                         mxz, myz, sz});
30406         assertEquals(calibrator.getInitialMa(), ma1);
30407         final Matrix ma2 = new Matrix(3, 3);
30408         calibrator.getInitialMa(ma2);
30409         assertEquals(ma1, ma2);
30410         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30411         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30412         final NEDPosition nedPosition1 = new NEDPosition();
30413         assertTrue(calibrator.getNedPosition(nedPosition1));
30414         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30415         assertNull(calibrator.getMeasurements());
30416         assertTrue(calibrator.isCommonAxisUsed());
30417         assertSame(calibrator.getListener(), this);
30418         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30419         assertFalse(calibrator.isReady());
30420         assertFalse(calibrator.isRunning());
30421         assertNull(calibrator.getEstimatedBiases());
30422         assertFalse(calibrator.getEstimatedBiases(null));
30423         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30424         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30425         assertNull(calibrator.getEstimatedBiasFx());
30426         assertNull(calibrator.getEstimatedBiasFy());
30427         assertNull(calibrator.getEstimatedBiasFz());
30428         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30429         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30430         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30431         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30432         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30433         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30434         assertNull(calibrator.getEstimatedMa());
30435         assertNull(calibrator.getEstimatedSx());
30436         assertNull(calibrator.getEstimatedSy());
30437         assertNull(calibrator.getEstimatedSz());
30438         assertNull(calibrator.getEstimatedMxy());
30439         assertNull(calibrator.getEstimatedMxz());
30440         assertNull(calibrator.getEstimatedMyx());
30441         assertNull(calibrator.getEstimatedMyz());
30442         assertNull(calibrator.getEstimatedMzx());
30443         assertNull(calibrator.getEstimatedMzy());
30444         assertNull(calibrator.getEstimatedCovariance());
30445         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30446         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30447                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30448         assertNotNull(calibrator.getGroundTruthGravityNorm());
30449         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30450         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30451         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30452                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30453         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30454         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30455         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30456 
30457         // Force IllegalArgumentException
30458         calibrator = null;
30459         try {
30460             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30461                     true, new Matrix(1, 1), ma, this);
30462             fail("IllegalArgumentException expected but not thrown");
30463         } catch (final IllegalArgumentException ignore) {
30464         }
30465         try {
30466             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30467                     true, new Matrix(1, 3), ma, this);
30468             fail("IllegalArgumentException expected but not thrown");
30469         } catch (final IllegalArgumentException ignore) {
30470         }
30471         try {
30472             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30473                     true, ba, new Matrix(1, 3), this);
30474             fail("IllegalArgumentException expected but not thrown");
30475         } catch (final IllegalArgumentException ignore) {
30476         }
30477         try {
30478             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30479                     true, ba, new Matrix(3, 1), this);
30480             fail("IllegalArgumentException expected but not thrown");
30481         } catch (final IllegalArgumentException ignore) {
30482         }
30483         assertNull(calibrator);
30484     }
30485 
30486     @Test
30487     public void testConstructor233() throws WrongSizeException {
30488         final Collection<StandardDeviationBodyKinematics> measurements =
30489                 Collections.emptyList();
30490 
30491         final Matrix ba = generateBa();
30492         final double[] bias = ba.getBuffer();
30493         final double biasX = ba.getElementAtIndex(0);
30494         final double biasY = ba.getElementAtIndex(1);
30495         final double biasZ = ba.getElementAtIndex(2);
30496 
30497         final Matrix ma = generateMaCommonAxis();
30498         final double sx = ma.getElementAt(0, 0);
30499         final double sy = ma.getElementAt(1, 1);
30500         final double sz = ma.getElementAt(2, 2);
30501         final double mxy = ma.getElementAt(0, 1);
30502         final double mxz = ma.getElementAt(0, 2);
30503         final double myx = ma.getElementAt(1, 0);
30504         final double myz = ma.getElementAt(1, 2);
30505         final double mzx = ma.getElementAt(2, 0);
30506         final double mzy = ma.getElementAt(2, 1);
30507 
30508         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30509         final double latitude = Math.toRadians(
30510                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30511         final double longitude = Math.toRadians(
30512                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30513         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30514         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30515         final NEDVelocity nedVelocity = new NEDVelocity();
30516         final ECEFPosition ecefPosition = new ECEFPosition();
30517         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30518         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30519                 ecefPosition, ecefVelocity);
30520 
30521         KnownPositionAccelerometerCalibrator calibrator =
30522                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
30523                         true, ba, ma);
30524 
30525         // check default values
30526         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30527         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30528         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30529         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30530         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30531         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30532         final Acceleration bx2 = new Acceleration(0.0,
30533                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30534         calibrator.getInitialBiasXAsAcceleration(bx2);
30535         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30536         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30537         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30538         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30539         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30540         final Acceleration by2 = new Acceleration(0.0,
30541                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30542         calibrator.getInitialBiasYAsAcceleration(by2);
30543         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30544         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30545         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30546         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30547         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30548         final Acceleration bz2 = new Acceleration(0.0,
30549                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30550         calibrator.getInitialBiasZAsAcceleration(bz2);
30551         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30552         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30553         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30554         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30555         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30556         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30557         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30558         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30559         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30560         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30561         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30562         final double[] bias1 = calibrator.getInitialBias();
30563         assertArrayEquals(bias1, bias, 0.0);
30564         final double[] bias2 = new double[3];
30565         calibrator.getInitialBias(bias2);
30566         assertArrayEquals(bias1, bias2, 0.0);
30567         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30568         assertEquals(b1, ba);
30569         final Matrix b2 = new Matrix(3, 1);
30570         calibrator.getInitialBiasAsMatrix(b2);
30571         assertEquals(b1, b2);
30572         final Matrix ma1 = new Matrix(3, 3);
30573         ma1.setSubmatrix(0, 0,
30574                 2, 2,
30575                 new double[]{sx, myx, mzx,
30576                         mxy, sy, mzy,
30577                         mxz, myz, sz});
30578         assertEquals(calibrator.getInitialMa(), ma1);
30579         final Matrix ma2 = new Matrix(3, 3);
30580         calibrator.getInitialMa(ma2);
30581         assertEquals(ma1, ma2);
30582         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30583         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30584         final NEDPosition nedPosition1 = new NEDPosition();
30585         assertTrue(calibrator.getNedPosition(nedPosition1));
30586         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30587         assertSame(calibrator.getMeasurements(), measurements);
30588         assertTrue(calibrator.isCommonAxisUsed());
30589         assertNull(calibrator.getListener());
30590         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30591         assertFalse(calibrator.isReady());
30592         assertFalse(calibrator.isRunning());
30593         assertNull(calibrator.getEstimatedBiases());
30594         assertFalse(calibrator.getEstimatedBiases(null));
30595         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30596         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30597         assertNull(calibrator.getEstimatedBiasFx());
30598         assertNull(calibrator.getEstimatedBiasFy());
30599         assertNull(calibrator.getEstimatedBiasFz());
30600         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30601         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30602         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30603         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30604         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30605         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30606         assertNull(calibrator.getEstimatedMa());
30607         assertNull(calibrator.getEstimatedSx());
30608         assertNull(calibrator.getEstimatedSy());
30609         assertNull(calibrator.getEstimatedSz());
30610         assertNull(calibrator.getEstimatedMxy());
30611         assertNull(calibrator.getEstimatedMxz());
30612         assertNull(calibrator.getEstimatedMyx());
30613         assertNull(calibrator.getEstimatedMyz());
30614         assertNull(calibrator.getEstimatedMzx());
30615         assertNull(calibrator.getEstimatedMzy());
30616         assertNull(calibrator.getEstimatedCovariance());
30617         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30618         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30619                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30620         assertNotNull(calibrator.getGroundTruthGravityNorm());
30621         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30622         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30623         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30624                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30625         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30626         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30627         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30628 
30629         // Force IllegalArgumentException
30630         calibrator = null;
30631         try {
30632             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30633                     measurements, true,
30634                     new Matrix(1, 1), ma);
30635             fail("IllegalArgumentException expected but not thrown");
30636         } catch (final IllegalArgumentException ignore) {
30637         }
30638         try {
30639             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30640                     measurements, true,
30641                     new Matrix(1, 3), ma);
30642             fail("IllegalArgumentException expected but not thrown");
30643         } catch (final IllegalArgumentException ignore) {
30644         }
30645         try {
30646             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30647                     measurements, true,
30648                     ba, new Matrix(1, 3));
30649             fail("IllegalArgumentException expected but not thrown");
30650         } catch (final IllegalArgumentException ignore) {
30651         }
30652         try {
30653             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30654                     measurements, true,
30655                     ba, new Matrix(3, 1));
30656             fail("IllegalArgumentException expected but not thrown");
30657         } catch (final IllegalArgumentException ignore) {
30658         }
30659         assertNull(calibrator);
30660     }
30661 
30662     @Test
30663     public void testConstructor234() throws WrongSizeException {
30664         final Collection<StandardDeviationBodyKinematics> measurements =
30665                 Collections.emptyList();
30666 
30667         final Matrix ba = generateBa();
30668         final double[] bias = ba.getBuffer();
30669         final double biasX = ba.getElementAtIndex(0);
30670         final double biasY = ba.getElementAtIndex(1);
30671         final double biasZ = ba.getElementAtIndex(2);
30672 
30673         final Matrix ma = generateMaCommonAxis();
30674         final double sx = ma.getElementAt(0, 0);
30675         final double sy = ma.getElementAt(1, 1);
30676         final double sz = ma.getElementAt(2, 2);
30677         final double mxy = ma.getElementAt(0, 1);
30678         final double mxz = ma.getElementAt(0, 2);
30679         final double myx = ma.getElementAt(1, 0);
30680         final double myz = ma.getElementAt(1, 2);
30681         final double mzx = ma.getElementAt(2, 0);
30682         final double mzy = ma.getElementAt(2, 1);
30683 
30684         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30685         final double latitude = Math.toRadians(
30686                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30687         final double longitude = Math.toRadians(
30688                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30689         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30690         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30691         final NEDVelocity nedVelocity = new NEDVelocity();
30692         final ECEFPosition ecefPosition = new ECEFPosition();
30693         final ECEFVelocity ecefVelocity = new ECEFVelocity();
30694         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30695                 ecefPosition, ecefVelocity);
30696 
30697         KnownPositionAccelerometerCalibrator calibrator =
30698                 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
30699                         true, ba, ma, this);
30700 
30701         // check default values
30702         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30703         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30704         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30705         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30706         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30707         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30708         final Acceleration bx2 = new Acceleration(0.0,
30709                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30710         calibrator.getInitialBiasXAsAcceleration(bx2);
30711         assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30712         assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30713         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30714         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30715         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30716         final Acceleration by2 = new Acceleration(0.0,
30717                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30718         calibrator.getInitialBiasYAsAcceleration(by2);
30719         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30720         assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30721         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30722         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30723         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30724         final Acceleration bz2 = new Acceleration(0.0,
30725                 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30726         calibrator.getInitialBiasZAsAcceleration(bz2);
30727         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30728         assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30729         assertEquals(calibrator.getInitialSx(), sx, 0.0);
30730         assertEquals(calibrator.getInitialSy(), sy, 0.0);
30731         assertEquals(calibrator.getInitialSz(), sz, 0.0);
30732         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30733         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30734         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30735         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30736         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30737         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30738         final double[] bias1 = calibrator.getInitialBias();
30739         assertArrayEquals(bias1, bias, 0.0);
30740         final double[] bias2 = new double[3];
30741         calibrator.getInitialBias(bias2);
30742         assertArrayEquals(bias1, bias2, 0.0);
30743         final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30744         assertEquals(b1, ba);
30745         final Matrix b2 = new Matrix(3, 1);
30746         calibrator.getInitialBiasAsMatrix(b2);
30747         assertEquals(b1, b2);
30748         final Matrix ma1 = new Matrix(3, 3);
30749         ma1.setSubmatrix(0, 0,
30750                 2, 2,
30751                 new double[]{sx, myx, mzx,
30752                         mxy, sy, mzy,
30753                         mxz, myz, sz});
30754         assertEquals(calibrator.getInitialMa(), ma1);
30755         final Matrix ma2 = new Matrix(3, 3);
30756         calibrator.getInitialMa(ma2);
30757         assertEquals(ma1, ma2);
30758         assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30759         assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30760         final NEDPosition nedPosition1 = new NEDPosition();
30761         assertTrue(calibrator.getNedPosition(nedPosition1));
30762         assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30763         assertSame(calibrator.getMeasurements(), measurements);
30764         assertTrue(calibrator.isCommonAxisUsed());
30765         assertSame(calibrator.getListener(), this);
30766         assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30767         assertFalse(calibrator.isReady());
30768         assertFalse(calibrator.isRunning());
30769         assertNull(calibrator.getEstimatedBiases());
30770         assertFalse(calibrator.getEstimatedBiases(null));
30771         assertNull(calibrator.getEstimatedBiasesAsMatrix());
30772         assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30773         assertNull(calibrator.getEstimatedBiasFx());
30774         assertNull(calibrator.getEstimatedBiasFy());
30775         assertNull(calibrator.getEstimatedBiasFz());
30776         assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30777         assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30778         assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30779         assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30780         assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30781         assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30782         assertNull(calibrator.getEstimatedMa());
30783         assertNull(calibrator.getEstimatedSx());
30784         assertNull(calibrator.getEstimatedSy());
30785         assertNull(calibrator.getEstimatedSz());
30786         assertNull(calibrator.getEstimatedMxy());
30787         assertNull(calibrator.getEstimatedMxz());
30788         assertNull(calibrator.getEstimatedMyx());
30789         assertNull(calibrator.getEstimatedMyz());
30790         assertNull(calibrator.getEstimatedMzx());
30791         assertNull(calibrator.getEstimatedMzy());
30792         assertNull(calibrator.getEstimatedCovariance());
30793         assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30794         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30795                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30796         assertNotNull(calibrator.getGroundTruthGravityNorm());
30797         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30798         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30799         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30800                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30801         final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30802         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30803         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30804 
30805         // Force IllegalArgumentException
30806         calibrator = null;
30807         try {
30808             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30809                     measurements, true,
30810                     new Matrix(1, 1), ma, this);
30811             fail("IllegalArgumentException expected but not thrown");
30812         } catch (final IllegalArgumentException ignore) {
30813         }
30814         try {
30815             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30816                     measurements, true,
30817                     new Matrix(1, 3), ma, this);
30818             fail("IllegalArgumentException expected but not thrown");
30819         } catch (final IllegalArgumentException ignore) {
30820         }
30821         try {
30822             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30823                     measurements, true,
30824                     ba, new Matrix(1, 3), this);
30825             fail("IllegalArgumentException expected but not thrown");
30826         } catch (final IllegalArgumentException ignore) {
30827         }
30828         try {
30829             calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30830                     measurements, true,
30831                     ba, new Matrix(3, 1), this);
30832             fail("IllegalArgumentException expected but not thrown");
30833         } catch (final IllegalArgumentException ignore) {
30834         }
30835         assertNull(calibrator);
30836     }
30837 
30838     @Test
30839     public void testGetSetInitialBiasX() throws LockedException {
30840         final KnownPositionAccelerometerCalibrator calibrator =
30841                 new KnownPositionAccelerometerCalibrator();
30842 
30843         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
30844 
30845         // set new value
30846         final Matrix ba = generateBa();
30847         final double biasX = ba.getElementAtIndex(0);
30848 
30849         calibrator.setInitialBiasX(biasX);
30850 
30851         // check
30852         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30853     }
30854 
30855     @Test
30856     public void testGetSetInitialBiasY() throws LockedException {
30857         final KnownPositionAccelerometerCalibrator calibrator =
30858                 new KnownPositionAccelerometerCalibrator();
30859 
30860         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
30861 
30862         // set new value
30863         final Matrix ba = generateBa();
30864         final double biasY = ba.getElementAtIndex(1);
30865 
30866         calibrator.setInitialBiasY(biasY);
30867 
30868         // check
30869         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30870     }
30871 
30872     @Test
30873     public void testGetSetInitialBiasZ() throws LockedException {
30874         final KnownPositionAccelerometerCalibrator calibrator =
30875                 new KnownPositionAccelerometerCalibrator();
30876 
30877         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
30878 
30879         // set new value
30880         final Matrix ba = generateBa();
30881         final double biasZ = ba.getElementAtIndex(2);
30882 
30883         calibrator.setInitialBiasZ(biasZ);
30884 
30885         // check
30886         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30887     }
30888 
30889     @Test
30890     public void testGetSetInitialBiasXAsAcceleration() throws LockedException {
30891         final KnownPositionAccelerometerCalibrator calibrator =
30892                 new KnownPositionAccelerometerCalibrator();
30893 
30894         // check default value
30895         final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30896 
30897         assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
30898         assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30899 
30900         // set new value
30901         final Matrix ba = generateBa();
30902         final double biasX = ba.getElementAtIndex(0);
30903 
30904         final Acceleration bx2 = new Acceleration(biasX,
30905                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30906         calibrator.setInitialBiasX(bx2);
30907 
30908         // check
30909         final Acceleration bx3 = calibrator.getInitialBiasXAsAcceleration();
30910         final Acceleration bx4 = new Acceleration(biasX,
30911                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30912         calibrator.getInitialBiasXAsAcceleration(bx4);
30913 
30914         assertEquals(bx3, bx4);
30915     }
30916 
30917     @Test
30918     public void testGetSetInitialBiasYAsAcceleration() throws LockedException {
30919         final KnownPositionAccelerometerCalibrator calibrator =
30920                 new KnownPositionAccelerometerCalibrator();
30921 
30922         // check default value
30923         final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30924 
30925         assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
30926         assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30927 
30928         // set new value
30929         final Matrix ba = generateBa();
30930         final double biasY = ba.getElementAtIndex(1);
30931 
30932         final Acceleration by2 = new Acceleration(biasY,
30933                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30934         calibrator.setInitialBiasY(by2);
30935 
30936         // check
30937         final Acceleration by3 = calibrator.getInitialBiasYAsAcceleration();
30938         final Acceleration by4 = new Acceleration(biasY,
30939                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30940         calibrator.getInitialBiasYAsAcceleration(by4);
30941 
30942         assertEquals(by3, by4);
30943     }
30944 
30945     @Test
30946     public void testGetSetInitialBiasZAsAcceleration() throws LockedException {
30947         final KnownPositionAccelerometerCalibrator calibrator =
30948                 new KnownPositionAccelerometerCalibrator();
30949 
30950         // check default value
30951         final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30952 
30953         assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
30954         assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30955 
30956         // set new value
30957         final Matrix ba = generateBa();
30958         final double biasZ = ba.getElementAtIndex(2);
30959 
30960         final Acceleration bz2 = new Acceleration(biasZ,
30961                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30962         calibrator.setInitialBiasZ(bz2);
30963 
30964         // check
30965         final Acceleration bz3 = calibrator.getInitialBiasZAsAcceleration();
30966         final Acceleration bz4 = new Acceleration(biasZ,
30967                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30968         calibrator.getInitialBiasZAsAcceleration(bz4);
30969 
30970         assertEquals(bz3, bz4);
30971     }
30972 
30973     @Test
30974     public void testSetInitialBias1() throws LockedException {
30975         final KnownPositionAccelerometerCalibrator calibrator =
30976                 new KnownPositionAccelerometerCalibrator();
30977 
30978         // check default values
30979         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
30980         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
30981         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
30982 
30983         // set new values
30984         final Matrix ba = generateBa();
30985         final double biasX = ba.getElementAtIndex(0);
30986         final double biasY = ba.getElementAtIndex(1);
30987         final double biasZ = ba.getElementAtIndex(2);
30988 
30989         calibrator.setInitialBias(biasX, biasY, biasZ);
30990 
30991         // check
30992         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30993         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30994         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30995     }
30996 
30997     @Test
30998     public void testSetInitialBias2() throws LockedException {
30999         final KnownPositionAccelerometerCalibrator calibrator =
31000                 new KnownPositionAccelerometerCalibrator();
31001 
31002         // check default values
31003         assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
31004         assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
31005         assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
31006 
31007         // set new values
31008         final Matrix ba = generateBa();
31009         final double biasX = ba.getElementAtIndex(0);
31010         final double biasY = ba.getElementAtIndex(1);
31011         final double biasZ = ba.getElementAtIndex(2);
31012 
31013         final Acceleration bx = new Acceleration(biasX,
31014                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31015         final Acceleration by = new Acceleration(biasY,
31016                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31017         final Acceleration bz = new Acceleration(biasZ,
31018                 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31019 
31020         calibrator.setInitialBias(bx, by, bz);
31021 
31022         // check
31023         assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31024         assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31025         assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31026     }
31027 
31028     @Test
31029     public void testGetSetInitialSx() throws WrongSizeException, LockedException {
31030         final KnownPositionAccelerometerCalibrator calibrator =
31031                 new KnownPositionAccelerometerCalibrator();
31032 
31033         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
31034 
31035         // set new value
31036         final Matrix ma = generateMaGeneral();
31037         final double sx = ma.getElementAt(0, 0);
31038 
31039         calibrator.setInitialSx(sx);
31040 
31041         // check
31042         assertEquals(calibrator.getInitialSx(), sx, 0.0);
31043     }
31044 
31045     @Test
31046     public void testGetSetInitialSy() throws WrongSizeException, LockedException {
31047         final KnownPositionAccelerometerCalibrator calibrator =
31048                 new KnownPositionAccelerometerCalibrator();
31049 
31050         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
31051 
31052         // set new value
31053         final Matrix ma = generateMaGeneral();
31054         final double sy = ma.getElementAt(1, 1);
31055 
31056         calibrator.setInitialSy(sy);
31057 
31058         // check
31059         assertEquals(calibrator.getInitialSy(), sy, 0.0);
31060     }
31061 
31062     @Test
31063     public void testGetSetInitialSz() throws WrongSizeException, LockedException {
31064         final KnownPositionAccelerometerCalibrator calibrator =
31065                 new KnownPositionAccelerometerCalibrator();
31066 
31067         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
31068 
31069         // set new value
31070         final Matrix ma = generateMaGeneral();
31071         final double sz = ma.getElementAt(2, 2);
31072 
31073         calibrator.setInitialSz(sz);
31074 
31075         // check
31076         assertEquals(calibrator.getInitialSz(), sz, 0.0);
31077     }
31078 
31079     @Test
31080     public void testGetSetInitialMxy() throws WrongSizeException, LockedException {
31081         final KnownPositionAccelerometerCalibrator calibrator =
31082                 new KnownPositionAccelerometerCalibrator();
31083 
31084         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
31085 
31086         // set new value
31087         final Matrix ma = generateMaGeneral();
31088         final double mxy = ma.getElementAt(0, 1);
31089 
31090         calibrator.setInitialMxy(mxy);
31091 
31092         // check
31093         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31094     }
31095 
31096     @Test
31097     public void testGetSetInitialMxz() throws WrongSizeException, LockedException {
31098         final KnownPositionAccelerometerCalibrator calibrator =
31099                 new KnownPositionAccelerometerCalibrator();
31100 
31101         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
31102 
31103         // set new value
31104         final Matrix ma = generateMaGeneral();
31105         final double mxz = ma.getElementAt(0, 2);
31106 
31107         calibrator.setInitialMxz(mxz);
31108 
31109         // check
31110         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31111     }
31112 
31113     @Test
31114     public void testGetSetInitialMyx() throws WrongSizeException, LockedException {
31115         final KnownPositionAccelerometerCalibrator calibrator =
31116                 new KnownPositionAccelerometerCalibrator();
31117 
31118         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
31119 
31120         // set new value
31121         final Matrix ma = generateMaGeneral();
31122         final double myx = ma.getElementAt(1, 0);
31123 
31124         calibrator.setInitialMyx(myx);
31125 
31126         // check
31127         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31128     }
31129 
31130     @Test
31131     public void testGetSetInitialMyz() throws WrongSizeException, LockedException {
31132         final KnownPositionAccelerometerCalibrator calibrator =
31133                 new KnownPositionAccelerometerCalibrator();
31134 
31135         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
31136 
31137         // set new value
31138         final Matrix ma = generateMaGeneral();
31139         final double myz = ma.getElementAt(1, 2);
31140 
31141         calibrator.setInitialMyz(myz);
31142 
31143         // check
31144         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31145     }
31146 
31147     @Test
31148     public void testGetSetInitialMzx() throws WrongSizeException, LockedException {
31149         final KnownPositionAccelerometerCalibrator calibrator =
31150                 new KnownPositionAccelerometerCalibrator();
31151 
31152         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
31153 
31154         // set new value
31155         final Matrix ma = generateMaGeneral();
31156         final double mzx = ma.getElementAt(2, 0);
31157 
31158         calibrator.setInitialMzx(mzx);
31159 
31160         // check
31161         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31162     }
31163 
31164     @Test
31165     public void testGetSetInitialMzy() throws WrongSizeException, LockedException {
31166         final KnownPositionAccelerometerCalibrator calibrator =
31167                 new KnownPositionAccelerometerCalibrator();
31168 
31169         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
31170 
31171         // set new value
31172         final Matrix ma = generateMaGeneral();
31173         final double mzy = ma.getElementAt(2, 1);
31174 
31175         calibrator.setInitialMzy(mzy);
31176 
31177         // check
31178         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31179     }
31180 
31181     @Test
31182     public void testSetInitialScalingFactors()
31183             throws WrongSizeException, LockedException {
31184 
31185         final KnownPositionAccelerometerCalibrator calibrator =
31186                 new KnownPositionAccelerometerCalibrator();
31187 
31188         // check default values
31189         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
31190         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
31191         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
31192 
31193         // set new values
31194         final Matrix ma = generateMaGeneral();
31195         final double sx = ma.getElementAt(0, 0);
31196         final double sy = ma.getElementAt(1, 1);
31197         final double sz = ma.getElementAt(2, 2);
31198 
31199         calibrator.setInitialScalingFactors(sx, sy, sz);
31200 
31201         // check
31202         assertEquals(calibrator.getInitialSx(), sx, 0.0);
31203         assertEquals(calibrator.getInitialSy(), sy, 0.0);
31204         assertEquals(calibrator.getInitialSz(), sz, 0.0);
31205     }
31206 
31207     @Test
31208     public void testSetInitialCrossCouplingErrors()
31209             throws WrongSizeException, LockedException {
31210 
31211         final KnownPositionAccelerometerCalibrator calibrator =
31212                 new KnownPositionAccelerometerCalibrator();
31213 
31214         // check default values
31215         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
31216         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
31217         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
31218         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
31219         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
31220         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
31221 
31222         // set new values
31223         final Matrix ma = generateMaGeneral();
31224         final double mxy = ma.getElementAt(0, 1);
31225         final double mxz = ma.getElementAt(0, 2);
31226         final double myx = ma.getElementAt(1, 0);
31227         final double myz = ma.getElementAt(1, 2);
31228         final double mzx = ma.getElementAt(2, 0);
31229         final double mzy = ma.getElementAt(2, 1);
31230 
31231         calibrator.setInitialCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
31232 
31233         // check
31234         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31235         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31236         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31237         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31238         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31239         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31240     }
31241 
31242     @Test
31243     public void testSetInitialScalingFactorsAndCrossCouplingErrors()
31244             throws WrongSizeException, LockedException {
31245 
31246         final KnownPositionAccelerometerCalibrator calibrator =
31247                 new KnownPositionAccelerometerCalibrator();
31248 
31249         // check default values
31250         assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
31251         assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
31252         assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
31253         assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
31254         assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
31255         assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
31256         assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
31257         assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
31258         assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
31259 
31260         // set new values
31261         final Matrix ma = generateMaGeneral();
31262         final double sx = ma.getElementAt(0, 0);
31263         final double sy = ma.getElementAt(1, 1);
31264         final double sz = ma.getElementAt(2, 2);
31265         final double mxy = ma.getElementAt(0, 1);
31266         final double mxz = ma.getElementAt(0, 2);
31267         final double myx = ma.getElementAt(1, 0);
31268         final double myz = ma.getElementAt(1, 2);
31269         final double mzx = ma.getElementAt(2, 0);
31270         final double mzy = ma.getElementAt(2, 1);
31271 
31272         calibrator.setInitialScalingFactorsAndCrossCouplingErrors(sx, sy, sz,
31273                 mxy, mxz, myx, myz, mzx, mzy);
31274 
31275         // check
31276         assertEquals(calibrator.getInitialSx(), sx, 0.0);
31277         assertEquals(calibrator.getInitialSy(), sy, 0.0);
31278         assertEquals(calibrator.getInitialSz(), sz, 0.0);
31279         assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31280         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31281         assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31282         assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31283         assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31284         assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31285     }
31286 
31287     @Test
31288     public void testGetSetInitialBias() throws LockedException {
31289         final KnownPositionAccelerometerCalibrator calibrator =
31290                 new KnownPositionAccelerometerCalibrator();
31291 
31292         // check default value
31293         final double[] bias1 = calibrator.getInitialBias();
31294         assertArrayEquals(bias1, new double[3], 0.0);
31295 
31296         // set new values
31297         final Matrix ba = generateBa();
31298         final double[] bias2 = ba.getBuffer();
31299         calibrator.setInitialBias(bias2);
31300 
31301         // check
31302         final double[] bias3 = calibrator.getInitialBias();
31303         final double[] bias4 = new double[3];
31304         calibrator.getInitialBias(bias4);
31305 
31306         assertArrayEquals(bias2, bias3, 0.0);
31307         assertArrayEquals(bias2, bias4, 0.0);
31308 
31309         // Force IllegalArgumentException
31310         try {
31311             calibrator.getInitialBias(new double[1]);
31312             fail("IllegalArgumentException expected but not thrown");
31313         } catch (final IllegalArgumentException ignore) {
31314         }
31315         try {
31316             calibrator.setInitialBias(new double[1]);
31317             fail("IllegalArgumentException expected but not thrown");
31318         } catch (final IllegalArgumentException ignore) {
31319         }
31320     }
31321 
31322     @Test
31323     public void testGetSetInitialBiasAsMatrix() throws WrongSizeException,
31324             LockedException {
31325 
31326         final KnownPositionAccelerometerCalibrator calibrator =
31327                 new KnownPositionAccelerometerCalibrator();
31328 
31329         // check default value
31330         final Matrix bias1 = calibrator.getInitialBiasAsMatrix();
31331         assertEquals(bias1, new Matrix(3, 1));
31332 
31333         // set new value
31334         final Matrix bias2 = generateBa();
31335         calibrator.setInitialBias(bias2);
31336 
31337         // check
31338         final Matrix bias3 = calibrator.getInitialBiasAsMatrix();
31339         final Matrix bias4 = new Matrix(3, 1);
31340         calibrator.getInitialBiasAsMatrix(bias4);
31341 
31342         assertEquals(bias2, bias3);
31343         assertEquals(bias2, bias4);
31344 
31345         // Force IllegalArgumentException
31346         try {
31347             calibrator.getInitialBiasAsMatrix(new Matrix(1, 1));
31348             fail("IllegalArgumentException expected but not thrown");
31349         } catch (final IllegalArgumentException ignore) {
31350         }
31351         try {
31352             calibrator.getInitialBiasAsMatrix(new Matrix(3, 3));
31353             fail("IllegalArgumentException expected but not thrown");
31354         } catch (final IllegalArgumentException ignore) {
31355         }
31356         try {
31357             calibrator.setInitialBias(new Matrix(1, 1));
31358             fail("IllegalArgumentException expected but not thrown");
31359         } catch (final IllegalArgumentException ignore) {
31360         }
31361         try {
31362             calibrator.setInitialBias(new Matrix(3, 3));
31363             fail("IllegalArgumentException expected but not thrown");
31364         } catch (final IllegalArgumentException ignore) {
31365         }
31366     }
31367 
31368     @Test
31369     public void testGetSetInitialMa() throws WrongSizeException, LockedException {
31370         final KnownPositionAccelerometerCalibrator calibrator =
31371                 new KnownPositionAccelerometerCalibrator();
31372 
31373         // test default value
31374         final Matrix ma1 = calibrator.getInitialMa();
31375         assertEquals(ma1, new Matrix(3, 3));
31376 
31377         // set new value
31378         final Matrix ma2 = generateMaGeneral();
31379         calibrator.setInitialMa(ma2);
31380 
31381         // check
31382         final Matrix ma3 = calibrator.getInitialMa();
31383         final Matrix ma4 = new Matrix(3, 3);
31384         calibrator.getInitialMa(ma4);
31385 
31386         assertEquals(ma2, ma3);
31387         assertEquals(ma2, ma4);
31388 
31389         // Force IllegalArgumentException
31390         try {
31391             calibrator.getInitialMa(new Matrix(1, 3));
31392             fail("IllegalArgumentException expected but not thrown");
31393         } catch (final IllegalArgumentException ignore) {
31394         }
31395         try {
31396             calibrator.getInitialMa(new Matrix(3, 1));
31397             fail("IllegalArgumentException expected but not thrown");
31398         } catch (final IllegalArgumentException ignore) {
31399         }
31400         try {
31401             calibrator.setInitialMa(new Matrix(1, 3));
31402             fail("IllegalArgumentException expected but not thrown");
31403         } catch (final IllegalArgumentException ignore) {
31404         }
31405         try {
31406             calibrator.setInitialMa(new Matrix(3, 1));
31407             fail("IllegalArgumentException expected but not thrown");
31408         } catch (final IllegalArgumentException ignore) {
31409         }
31410     }
31411 
31412     @Test
31413     public void testGetSetEcefPosition() throws LockedException {
31414         final KnownPositionAccelerometerCalibrator calibrator =
31415                 new KnownPositionAccelerometerCalibrator();
31416 
31417         // check default value
31418         assertNull(calibrator.getEcefPosition());
31419 
31420         // set new value
31421         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31422         final double latitude = Math.toRadians(
31423                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31424         final double longitude = Math.toRadians(
31425                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31426         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31427         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31428         final NEDVelocity nedVelocity = new NEDVelocity();
31429         final ECEFPosition ecefPosition = new ECEFPosition();
31430         final ECEFVelocity ecefVelocity = new ECEFVelocity();
31431         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
31432                 ecefPosition, ecefVelocity);
31433 
31434         calibrator.setPosition(ecefPosition);
31435 
31436         // check
31437         assertSame(calibrator.getEcefPosition(), ecefPosition);
31438     }
31439 
31440     @Test
31441     public void testGetSetNedPosition() throws LockedException {
31442         final KnownPositionAccelerometerCalibrator calibrator =
31443                 new KnownPositionAccelerometerCalibrator();
31444 
31445         // check default value
31446         assertNull(calibrator.getNedPosition());
31447         assertFalse(calibrator.getNedPosition(null));
31448 
31449         // set new value
31450         final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31451         final double latitude = Math.toRadians(
31452                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31453         final double longitude = Math.toRadians(
31454                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31455         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31456         final NEDPosition nedPosition1 = new NEDPosition(latitude, longitude, height);
31457 
31458         calibrator.setPosition(nedPosition1);
31459 
31460         // check
31461         final NEDPosition nedPosition2 = calibrator.getNedPosition();
31462         final NEDPosition nedPosition3 = new NEDPosition();
31463         calibrator.getNedPosition(nedPosition3);
31464 
31465         assertTrue(nedPosition1.equals(nedPosition2, ABSOLUTE_ERROR));
31466         assertTrue(nedPosition1.equals(nedPosition3, ABSOLUTE_ERROR));
31467     }
31468 
31469     @Test
31470     public void testGetSetMeasurements() throws LockedException {
31471         final KnownPositionAccelerometerCalibrator calibrator =
31472                 new KnownPositionAccelerometerCalibrator();
31473 
31474         // check default value
31475         assertNull(calibrator.getMeasurements());
31476 
31477         // set new value
31478         final Collection<StandardDeviationBodyKinematics> measurements =
31479                 Collections.emptyList();
31480         calibrator.setMeasurements(measurements);
31481 
31482         // check
31483         assertSame(calibrator.getMeasurements(), measurements);
31484     }
31485 
31486     @Test
31487     public void testIsSetCommonAxisUsed() throws LockedException {
31488         final KnownPositionAccelerometerCalibrator calibrator =
31489                 new KnownPositionAccelerometerCalibrator();
31490 
31491         // check default value
31492         assertFalse(calibrator.isCommonAxisUsed());
31493 
31494         // set new value
31495         calibrator.setCommonAxisUsed(true);
31496 
31497         // check
31498         assertTrue(calibrator.isCommonAxisUsed());
31499     }
31500 
31501     @Test
31502     public void testGetSetListener() throws LockedException {
31503         final KnownPositionAccelerometerCalibrator calibrator =
31504                 new KnownPositionAccelerometerCalibrator();
31505 
31506         // check default value
31507         assertNull(calibrator.getListener());
31508 
31509         // set new value
31510         calibrator.setListener(this);
31511 
31512         // check
31513         assertSame(calibrator.getListener(), this);
31514     }
31515 
31516     @Test
31517     public void testIsReady() throws LockedException {
31518         final KnownPositionAccelerometerCalibrator calibrator =
31519                 new KnownPositionAccelerometerCalibrator();
31520 
31521         // check
31522         assertFalse(calibrator.isReady());
31523 
31524 
31525         // set empty measurements
31526         final Collection<StandardDeviationBodyKinematics> measurements1 =
31527                 Collections.emptyList();
31528         calibrator.setMeasurements(measurements1);
31529 
31530         // check
31531         assertFalse(calibrator.isReady());
31532 
31533 
31534         // set enough measurements for general case
31535         calibrator.setCommonAxisUsed(false);
31536 
31537         final List<StandardDeviationBodyKinematics> measurements2 = new ArrayList<>();
31538         for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
31539             measurements2.add(new StandardDeviationBodyKinematics());
31540         }
31541         calibrator.setMeasurements(measurements2);
31542 
31543         // check
31544         assertFalse(calibrator.isReady());
31545 
31546 
31547         // set position
31548         final ECEFPosition position = new ECEFPosition();
31549         calibrator.setPosition(position);
31550 
31551         assertTrue(calibrator.isReady());
31552 
31553 
31554         // set enough measurements for common axis case
31555         measurements2.clear();
31556         for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
31557             measurements2.add(new StandardDeviationBodyKinematics());
31558         }
31559         calibrator.setMeasurements(measurements2);
31560 
31561         // check
31562         assertFalse(calibrator.isReady());
31563 
31564         calibrator.setCommonAxisUsed(true);
31565 
31566         assertTrue(calibrator.isReady());
31567     }
31568 
31569     @Test
31570     public void testCalibrateForGeneralCaseWithMinimumMeasuresAndNoNoise()
31571             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31572             LockedException, NotReadyException {
31573 
31574         int numValid = 0;
31575         for (int t = 0; t < TIMES; t++) {
31576             final Matrix ba = generateBa();
31577             final Matrix bg = generateBg();
31578             final Matrix ma = generateMaGeneral();
31579             final Matrix mg = generateMg();
31580             final Matrix gg = generateGg();
31581             // when using minimum number of measurements we must not add any noise so that
31582             // a solution is found, when adding more measurements, certain noise can be added
31583             final double accelNoiseRootPSD = 0.0;
31584             final double gyroNoiseRootPSD = 0.0;
31585             final double accelQuantLevel = 0.0;
31586             final double gyroQuantLevel = 0.0;
31587 
31588             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31589                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31590 
31591             final Random random = new Random();
31592             final UniformRandomizer randomizer = new UniformRandomizer(random);
31593             final double latitude = Math.toRadians(
31594                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31595             final double longitude = Math.toRadians(
31596                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31597             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31598             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31599 
31600             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31601             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31602             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31603 
31604             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31605             for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
31606                 final double roll = Math.toRadians(
31607                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31608                 final double pitch = Math.toRadians(
31609                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31610                 final double yaw = Math.toRadians(
31611                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31612                 final CoordinateTransformation nedC = new CoordinateTransformation(
31613                         roll, pitch, yaw, FrameType.BODY_FRAME,
31614                         FrameType.LOCAL_NAVIGATION_FRAME);
31615 
31616                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31617                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31618                         .convertNEDtoECEFAndReturnNew(nedFrame);
31619 
31620                 // compute ground-truth kinematics that should be generated at provided
31621                 // position, velocity and orientation
31622                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31623                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31624                                 ecefFrame);
31625 
31626                 // apply known calibration parameters to distort ground-truth and generate a
31627                 // measured kinematics sample
31628                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31629                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31630 
31631                 final StandardDeviationBodyKinematics measurement =
31632                         new StandardDeviationBodyKinematics(measuredKinematics,
31633                                 specificForceStandardDeviation,
31634                                 angularRateStandardDeviation);
31635                 measurements.add(measurement);
31636             }
31637 
31638             // When we have the minimum number of measurements, we need to provide
31639             // an initial solution close to the true solution
31640             final KnownPositionAccelerometerCalibrator calibrator =
31641                     new KnownPositionAccelerometerCalibrator(nedPosition,
31642                             measurements, false, ba, ma, this);
31643 
31644             // estimate
31645             reset();
31646             assertTrue(calibrator.isReady());
31647             assertFalse(calibrator.isRunning());
31648             assertEquals(mCalibrateStart, 0);
31649             assertEquals(mCalibrateEnd, 0);
31650 
31651             try {
31652                 calibrator.calibrate();
31653             } catch (final CalibrationException e) {
31654                 continue;
31655             }
31656 
31657             // check
31658             assertTrue(calibrator.isReady());
31659             assertFalse(calibrator.isRunning());
31660             assertEquals(mCalibrateStart, 1);
31661             assertEquals(mCalibrateEnd, 1);
31662 
31663             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
31664             final Matrix estimatedMa = calibrator.getEstimatedMa();
31665 
31666             if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
31667                 continue;
31668             }
31669             if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
31670                 continue;
31671             }
31672 
31673             assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
31674             assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
31675 
31676             numValid++;
31677 
31678             break;
31679         }
31680 
31681         assertTrue(numValid > 0);
31682     }
31683 
31684     @Test
31685     public void testCalibrateForGeneralCaseWithLargeNumberOfMeasurementsAndNoise()
31686             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31687             LockedException, NotReadyException {
31688 
31689         int numValid = 0;
31690         for (int t = 0; t < TIMES; t++) {
31691             final Matrix ba = generateBa();
31692             final Matrix bg = generateBg();
31693             final Matrix ma = generateMaGeneral();
31694             final Matrix mg = generateMg();
31695             final Matrix gg = generateGg();
31696             final double accelNoiseRootPSD = getAccelNoiseRootPSD();
31697             final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
31698             final double accelQuantLevel = 0.0;
31699             final double gyroQuantLevel = 0.0;
31700 
31701             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31702                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31703 
31704             final Random random = new Random();
31705             final UniformRandomizer randomizer = new UniformRandomizer(random);
31706             final double latitude = Math.toRadians(
31707                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31708             final double longitude = Math.toRadians(
31709                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31710             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31711             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31712 
31713             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31714             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31715             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31716 
31717             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31718             for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
31719                 final double roll = Math.toRadians(
31720                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31721                 final double pitch = Math.toRadians(
31722                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31723                 final double yaw = Math.toRadians(
31724                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31725                 final CoordinateTransformation nedC = new CoordinateTransformation(
31726                         roll, pitch, yaw, FrameType.BODY_FRAME,
31727                         FrameType.LOCAL_NAVIGATION_FRAME);
31728 
31729                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31730                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31731                         .convertNEDtoECEFAndReturnNew(nedFrame);
31732 
31733                 // compute ground-truth kinematics that should be generated at provided
31734                 // position, velocity and orientation
31735                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31736                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31737                                 ecefFrame);
31738 
31739                 // apply known calibration parameters to distort ground-truth and generate a
31740                 // measured kinematics sample
31741                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31742                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31743 
31744                 final StandardDeviationBodyKinematics measurement =
31745                         new StandardDeviationBodyKinematics(measuredKinematics,
31746                                 specificForceStandardDeviation,
31747                                 angularRateStandardDeviation);
31748                 measurements.add(measurement);
31749             }
31750 
31751             // When we have a large number of measurements, we do not need to provide
31752             // an initial solution as it will probably converge to true true solution
31753             final KnownPositionAccelerometerCalibrator calibrator =
31754                     new KnownPositionAccelerometerCalibrator(nedPosition,
31755                             measurements, false, this);
31756 
31757             // estimate
31758             reset();
31759             assertTrue(calibrator.isReady());
31760             assertFalse(calibrator.isRunning());
31761             assertEquals(mCalibrateStart, 0);
31762             assertEquals(mCalibrateEnd, 0);
31763 
31764             try {
31765                 calibrator.calibrate();
31766             } catch (final CalibrationException e) {
31767                 continue;
31768             }
31769 
31770             // check
31771             assertTrue(calibrator.isReady());
31772             assertFalse(calibrator.isRunning());
31773             assertEquals(mCalibrateStart, 1);
31774             assertEquals(mCalibrateEnd, 1);
31775 
31776             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
31777             final Matrix estimatedMa = calibrator.getEstimatedMa();
31778 
31779             if (!ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR)) {
31780                 continue;
31781             }
31782             if (!ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR)) {
31783                 continue;
31784             }
31785 
31786             assertTrue(ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR));
31787             assertTrue(ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR));
31788 
31789             numValid++;
31790 
31791             break;
31792         }
31793 
31794         assertTrue(numValid > 0);
31795     }
31796 
31797     @Test
31798     public void testCalibrateForCommonAxisCaseWithMinimumMeasuresAndNoNoise()
31799             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31800             LockedException, NotReadyException {
31801 
31802         int numValid = 0;
31803         for (int t = 0; t < TIMES; t++) {
31804             final Matrix ba = generateBa();
31805             final Matrix bg = generateBg();
31806             final Matrix ma = generateMaCommonAxis();
31807             final Matrix mg = generateMg();
31808             final Matrix gg = generateGg();
31809             // when using minimum number of measurements we must not add any noise so that
31810             // a solution is found, when adding more measurements, certain noise can be added
31811             final double accelNoiseRootPSD = 0.0;
31812             final double gyroNoiseRootPSD = 0.0;
31813             final double accelQuantLevel = 0.0;
31814             final double gyroQuantLevel = 0.0;
31815 
31816             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31817                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31818 
31819             final Random random = new Random();
31820             final UniformRandomizer randomizer = new UniformRandomizer(random);
31821             final double latitude = Math.toRadians(
31822                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31823             final double longitude = Math.toRadians(
31824                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31825             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31826             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31827 
31828             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31829             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31830             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31831 
31832             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31833             for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
31834                 final double roll = Math.toRadians(
31835                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31836                 final double pitch = Math.toRadians(
31837                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31838                 final double yaw = Math.toRadians(
31839                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31840                 final CoordinateTransformation nedC = new CoordinateTransformation(
31841                         roll, pitch, yaw, FrameType.BODY_FRAME,
31842                         FrameType.LOCAL_NAVIGATION_FRAME);
31843 
31844                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31845                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31846                         .convertNEDtoECEFAndReturnNew(nedFrame);
31847 
31848                 // compute ground-truth kinematics that should be generated at provided
31849                 // position, velocity and orientation
31850                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31851                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31852                                 ecefFrame);
31853 
31854                 // apply known calibration parameters to distort ground-truth and generate a
31855                 // measured kinematics sample
31856                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31857                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31858 
31859                 final StandardDeviationBodyKinematics measurement =
31860                         new StandardDeviationBodyKinematics(measuredKinematics,
31861                                 specificForceStandardDeviation,
31862                                 angularRateStandardDeviation);
31863                 measurements.add(measurement);
31864             }
31865 
31866             // When we have the minimum number of measurements, we need to provide
31867             // an initial solution close to the true solution
31868             final KnownPositionAccelerometerCalibrator calibrator =
31869                     new KnownPositionAccelerometerCalibrator(nedPosition,
31870                             measurements, true, ba, ma, this);
31871 
31872             // estimate
31873             reset();
31874             assertTrue(calibrator.isReady());
31875             assertFalse(calibrator.isRunning());
31876             assertEquals(mCalibrateStart, 0);
31877             assertEquals(mCalibrateEnd, 0);
31878 
31879             try {
31880                 calibrator.calibrate();
31881             } catch (final CalibrationException e) {
31882                 continue;
31883             }
31884 
31885             // check
31886             assertTrue(calibrator.isReady());
31887             assertFalse(calibrator.isRunning());
31888             assertEquals(mCalibrateStart, 1);
31889             assertEquals(mCalibrateEnd, 1);
31890 
31891             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
31892             final Matrix estimatedMa = calibrator.getEstimatedMa();
31893 
31894             if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
31895                 continue;
31896             }
31897             if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
31898                 continue;
31899             }
31900 
31901             assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
31902             assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
31903 
31904             numValid++;
31905 
31906             break;
31907         }
31908 
31909         assertTrue(numValid > 0);
31910     }
31911 
31912     @Test
31913     public void testCalibrateForCommonAxisCaseWithLargeNumberOfMeasurementsAndNoise()
31914             throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31915             LockedException, NotReadyException {
31916 
31917         int numValid = 0;
31918         for (int t = 0; t < TIMES; t++) {
31919             final Matrix ba = generateBa();
31920             final Matrix bg = generateBg();
31921             final Matrix ma = generateMaCommonAxis();
31922             final Matrix mg = generateMg();
31923             final Matrix gg = generateGg();
31924             final double accelNoiseRootPSD = getAccelNoiseRootPSD();
31925             final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
31926             final double accelQuantLevel = 0.0;
31927             final double gyroQuantLevel = 0.0;
31928 
31929             final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31930                     gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31931 
31932             final Random random = new Random();
31933             final UniformRandomizer randomizer = new UniformRandomizer(random);
31934             final double latitude = Math.toRadians(
31935                     randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31936             final double longitude = Math.toRadians(
31937                     randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31938             final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31939             final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31940 
31941             final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31942             final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31943             final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31944 
31945             final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31946             for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
31947                 final double roll = Math.toRadians(
31948                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31949                 final double pitch = Math.toRadians(
31950                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31951                 final double yaw = Math.toRadians(
31952                         randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31953                 final CoordinateTransformation nedC = new CoordinateTransformation(
31954                         roll, pitch, yaw, FrameType.BODY_FRAME,
31955                         FrameType.LOCAL_NAVIGATION_FRAME);
31956 
31957                 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31958                 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31959                         .convertNEDtoECEFAndReturnNew(nedFrame);
31960 
31961                 // compute ground-truth kinematics that should be generated at provided
31962                 // position, velocity and orientation
31963                 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31964                         .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31965                                 ecefFrame);
31966 
31967                 // apply known calibration parameters to distort ground-truth and generate a
31968                 // measured kinematics sample
31969                 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31970                         .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31971 
31972                 final StandardDeviationBodyKinematics measurement =
31973                         new StandardDeviationBodyKinematics(measuredKinematics,
31974                                 specificForceStandardDeviation,
31975                                 angularRateStandardDeviation);
31976                 measurements.add(measurement);
31977             }
31978 
31979             // When we have a large number of measurements, we do not need to provide
31980             // an initial solution as it will probably converge to true true solution
31981             final KnownPositionAccelerometerCalibrator calibrator =
31982                     new KnownPositionAccelerometerCalibrator(nedPosition,
31983                             measurements, true, this);
31984 
31985             // estimate
31986             reset();
31987             assertTrue(calibrator.isReady());
31988             assertFalse(calibrator.isRunning());
31989             assertEquals(mCalibrateStart, 0);
31990             assertEquals(mCalibrateEnd, 0);
31991 
31992             try {
31993                 calibrator.calibrate();
31994             } catch (final CalibrationException e) {
31995                 continue;
31996             }
31997 
31998             // check
31999             assertTrue(calibrator.isReady());
32000             assertFalse(calibrator.isRunning());
32001             assertEquals(mCalibrateStart, 1);
32002             assertEquals(mCalibrateEnd, 1);
32003 
32004             final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
32005             final Matrix estimatedMa = calibrator.getEstimatedMa();
32006 
32007             if (!ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR)) {
32008                 continue;
32009             }
32010             if (!ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR)) {
32011                 continue;
32012             }
32013 
32014             assertTrue(ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR));
32015             assertTrue(ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR));
32016 
32017             numValid++;
32018 
32019             break;
32020         }
32021 
32022         assertTrue(numValid > 0);
32023     }
32024 
32025     @Test
32026     public void testNorms() throws InvalidSourceAndDestinationFrameTypeException {
32027 
32028         // This test checks that the norm of true specific force and true angular
32029         // rates are equal to the gravity value (approximately 9.81 m/s^2), and the
32030         // Earth rotation rate respectively for any Earth location.
32031         // This property is the principle used for KnownPositionAccelerometerCalibrator
32032         // to be able to calibrate the accelerometer by taking kinemtics measurements
32033         // with unknown orientation, but known Earth location.
32034 
32035         final Random random = new Random();
32036         final UniformRandomizer randomizer = new UniformRandomizer(random);
32037         final double latitude = Math.toRadians(
32038                 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
32039         final double longitude = Math.toRadians(
32040                 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32041         final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32042         final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32043 
32044         final double roll = Math.toRadians(
32045                 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32046         final double pitch = Math.toRadians(
32047                 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32048         final double yaw = Math.toRadians(
32049                 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32050         final CoordinateTransformation nedC = new CoordinateTransformation(
32051                 roll, pitch, yaw, FrameType.BODY_FRAME,
32052                 FrameType.LOCAL_NAVIGATION_FRAME);
32053 
32054         final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
32055         final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
32056                 .convertNEDtoECEFAndReturnNew(nedFrame);
32057 
32058         final BodyKinematics trueKinematics = ECEFKinematicsEstimator
32059                 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
32060                         ecefFrame);
32061 
32062         final double fNorm = trueKinematics.getSpecificForceNorm();
32063         final double wNorm = trueKinematics.getAngularRateNorm();
32064 
32065         final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(ecefFrame);
32066         final double g = gravity.getNorm();
32067 
32068         assertEquals(g, fNorm, ABSOLUTE_ERROR);
32069         assertEquals(Constants.EARTH_ROTATION_RATE, wNorm, ABSOLUTE_ERROR);
32070     }
32071 
32072     @Override
32073     public void onCalibrateStart(
32074             final KnownPositionAccelerometerCalibrator calibrator) {
32075         checkLocked(calibrator);
32076         mCalibrateStart++;
32077     }
32078 
32079     @Override
32080     public void onCalibrateEnd(
32081             final KnownPositionAccelerometerCalibrator calibrator) {
32082         checkLocked(calibrator);
32083         mCalibrateEnd++;
32084     }
32085 
32086     private void reset() {
32087         mCalibrateStart = 0;
32088         mCalibrateEnd = 0;
32089     }
32090 
32091     private void checkLocked(final KnownPositionAccelerometerCalibrator calibrator) {
32092         assertTrue(calibrator.isRunning());
32093         try {
32094             calibrator.setInitialBiasX(0.0);
32095             fail("LockedException expected but not thrown");
32096         } catch (final LockedException ignore) {
32097         }
32098         try {
32099             calibrator.setInitialBiasY(0.0);
32100             fail("LockedException expected but not thrown");
32101         } catch (final LockedException ignore) {
32102         }
32103         try {
32104             calibrator.setInitialBiasZ(0.0);
32105             fail("LockedException expected but not thrown");
32106         } catch (final LockedException ignore) {
32107         }
32108         try {
32109             calibrator.setInitialBiasX(null);
32110             fail("LockedException expected but not thrown");
32111         } catch (final LockedException ignore) {
32112         }
32113         try {
32114             calibrator.setInitialBiasY(null);
32115             fail("LockedException expected but not thrown");
32116         } catch (final LockedException ignore) {
32117         }
32118         try {
32119             calibrator.setInitialBiasZ(null);
32120             fail("LockedException expected but not thrown");
32121         } catch (final LockedException ignore) {
32122         }
32123         try {
32124             calibrator.setInitialBias(0.0, 0.0, 0.0);
32125             fail("LockedException expected but not thrown");
32126         } catch (final LockedException ignore) {
32127         }
32128         try {
32129             calibrator.setInitialBias(null, null, null);
32130             fail("LockedException expected but not thrown");
32131         } catch (final LockedException ignore) {
32132         }
32133         try {
32134             calibrator.setInitialSx(0.0);
32135             fail("LockedException expected but not thrown");
32136         } catch (final LockedException ignore) {
32137         }
32138         try {
32139             calibrator.setInitialSy(0.0);
32140             fail("LockedException expected but not thrown");
32141         } catch (final LockedException ignore) {
32142         }
32143         try {
32144             calibrator.setInitialSz(0.0);
32145             fail("LockedException expected but not thrown");
32146         } catch (final LockedException ignore) {
32147         }
32148         try {
32149             calibrator.setInitialMxy(0.0);
32150             fail("LockedException expected but not thrown");
32151         } catch (final LockedException ignore) {
32152         }
32153         try {
32154             calibrator.setInitialMxz(0.0);
32155             fail("LockedException expected but not thrown");
32156         } catch (final LockedException ignore) {
32157         }
32158         try {
32159             calibrator.setInitialMyx(0.0);
32160             fail("LockedException expected but not thrown");
32161         } catch (final LockedException ignore) {
32162         }
32163         try {
32164             calibrator.setInitialMyz(0.0);
32165             fail("LockedException expected but not thrown");
32166         } catch (final LockedException ignore) {
32167         }
32168         try {
32169             calibrator.setInitialMzx(0.0);
32170             fail("LockedException expected but not thrown");
32171         } catch (final LockedException ignore) {
32172         }
32173         try {
32174             calibrator.setInitialMzy(0.0);
32175             fail("LockedException expected but not thrown");
32176         } catch (final LockedException ignore) {
32177         }
32178         try {
32179             calibrator.setInitialScalingFactors(0.0, 0.0, 0.0);
32180             fail("LockedException expected but not thrown");
32181         } catch (final LockedException ignore) {
32182         }
32183         try {
32184             calibrator.setInitialCrossCouplingErrors(
32185                     0.0, 0.0, 0.0,
32186                     0.0, 0.0, 0.0);
32187             fail("LockedException expected but not thrown");
32188         } catch (final LockedException ignore) {
32189         }
32190         try {
32191             calibrator.setInitialScalingFactorsAndCrossCouplingErrors(
32192                     0.0, 0.0, 0.0,
32193                     0.0, 0.0, 0.0,
32194                     0.0, 0.0, 0.0);
32195             fail("LockedException expected but not thrown");
32196         } catch (final LockedException ignore) {
32197         }
32198         try {
32199             calibrator.setInitialBias((double[]) null);
32200             fail("LockedException expected but not thrown");
32201         } catch (final LockedException ignore) {
32202         }
32203         try {
32204             calibrator.setInitialBias((Matrix) null);
32205             fail("LockedException expected but not thrown");
32206         } catch (final LockedException ignore) {
32207         }
32208         try {
32209             calibrator.setInitialMa(null);
32210             fail("LockedException expected but not thrown");
32211         } catch (final LockedException ignore) {
32212         }
32213         try {
32214             calibrator.setPosition((ECEFPosition) null);
32215             fail("LockedException expected but not thrown");
32216         } catch (final LockedException ignore) {
32217         }
32218         try {
32219             calibrator.setPosition((NEDPosition) null);
32220             fail("LockedException expected but not thrown");
32221         } catch (final LockedException ignore) {
32222         }
32223         try {
32224             calibrator.setMeasurements(null);
32225             fail("LockedException expected but not thrown");
32226         } catch (final LockedException ignore) {
32227         }
32228         try {
32229             calibrator.setCommonAxisUsed(true);
32230             fail("LockedException expected but not thrown");
32231         } catch (final LockedException ignore) {
32232         }
32233         try {
32234             calibrator.setListener(this);
32235             fail("LockedException expected but not thrown");
32236         } catch (final LockedException ignore) {
32237         }
32238         try {
32239             calibrator.calibrate();
32240             fail("LockedException expected but not thrown");
32241         } catch (final LockedException ignore) {
32242         } catch (final Exception e) {
32243             fail("LockedException expected but not thrown");
32244         }
32245     }
32246 
32247     private Matrix generateBa() {
32248         return Matrix.newFromArray(new double[]{
32249                 900 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
32250                 -1300 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
32251                 800 * MICRO_G_TO_METERS_PER_SECOND_SQUARED});
32252     }
32253 
32254     private Matrix generateBg() {
32255         return Matrix.newFromArray(new double[]{
32256                 -9 * DEG_TO_RAD / 3600.0,
32257                 13 * DEG_TO_RAD / 3600.0,
32258                 -8 * DEG_TO_RAD / 3600.0});
32259     }
32260 
32261     private Matrix generateMaGeneral() throws WrongSizeException {
32262         final Matrix result = new Matrix(3, 3);
32263         result.fromArray(new double[]{
32264                 500e-6, -300e-6, 200e-6,
32265                 -150e-6, -600e-6, 250e-6,
32266                 -250e-6, 100e-6, 450e-6
32267         }, false);
32268 
32269         return result;
32270     }
32271 
32272     private Matrix generateMaCommonAxis() throws WrongSizeException {
32273         final Matrix result = new Matrix(3, 3);
32274         result.fromArray(new double[]{
32275                 500e-6, -300e-6, 200e-6,
32276                 0.0, -600e-6, 250e-6,
32277                 0.0, 0.0, 450e-6
32278         }, false);
32279 
32280         return result;
32281     }
32282 
32283     private Matrix generateMg() throws WrongSizeException {
32284         final Matrix result = new Matrix(3, 3);
32285         result.fromArray(new double[]{
32286                 400e-6, -300e-6, 250e-6,
32287                 0.0, -300e-6, -150e-6,
32288                 0.0, 0.0, -350e-6
32289         }, false);
32290 
32291         return result;
32292     }
32293 
32294     private Matrix generateGg() throws WrongSizeException {
32295         final Matrix result = new Matrix(3, 3);
32296         final double tmp = DEG_TO_RAD / (3600 * 9.80665);
32297         result.fromArray(new double[]{
32298                 0.9 * tmp, -1.1 * tmp, -0.6 * tmp,
32299                 -0.5 * tmp, 1.9 * tmp, -1.6 * tmp,
32300                 0.3 * tmp, 1.1 * tmp, -1.3 * tmp
32301         }, false);
32302 
32303         return result;
32304     }
32305 
32306     private double getAccelNoiseRootPSD() {
32307         return 100.0 * MICRO_G_TO_METERS_PER_SECOND_SQUARED;
32308     }
32309 
32310     private double getGyroNoiseRootPSD() {
32311         return 0.01 * DEG_TO_RAD / 60.0;
32312     }
32313 }